diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 2da291678b1dc33cff6bd4f1fded2071881efd44..1eb08744636f0d2c785086c242aea35032383a10 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -31,9 +31,6 @@ #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. @@ -377,7 +374,8 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message // Create buffer uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; // Rewriting header to ensure correct link ID is set - if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len); + static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; + if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]); // Write message into buffer, prepending start sign int len = mavlink_msg_to_send_buffer(buffer, &message); // If link is connected diff --git a/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp b/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp index cdd3dfab3787c3aa10bfee494a782e83b2af1c5b..dcab6afb1ef5e055687dea3445cdf548dc4b0f4e 100644 --- a/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp +++ b/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp @@ -91,6 +91,7 @@ Size PlateCarreeProjection::GetTileMatrixMaxXY(const int &zoom) Size PlateCarreeProjection::GetTileMatrixMinXY(const int &zoom) { + Q_UNUSED(zoom); return Size(0, 0); } } diff --git a/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp b/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp index d1d24c5ccdb5d70d1b95f35734057ab1251d16ad..b41916aa4de0cf7df3e5efe77b7c5574daee1e78 100644 --- a/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp +++ b/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp @@ -90,6 +90,7 @@ Size PlateCarreeProjectionPergo::GetTileMatrixMaxXY(const int &zoom) Size PlateCarreeProjectionPergo::GetTileMatrixMinXY(const int &zoom) { + Q_UNUSED(zoom); return Size(0, 0); } } diff --git a/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp b/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp index a54417613fdc424c3301f328571ab4beedf52500..ac104aef23a91e46f74894806fd7b836633369a0 100644 --- a/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp +++ b/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp @@ -32,7 +32,15 @@ namespace mapcontrol { - MapGraphicItem::MapGraphicItem(internals::Core *core, Configuration *configuration):core(core),config(configuration),MapRenderTransform(1), maxZoom(17),minZoom(2),zoomReal(0),isSelected(false),rotation(0),zoomDigi(0) + MapGraphicItem::MapGraphicItem(internals::Core *core, Configuration *configuration):core(core), + config(configuration), + MapRenderTransform(1), + maxZoom(17), + minZoom(2), + zoomReal(0), + rotation(0), + zoomDigi(0), + isSelected(false) { dragons.load(QString::fromUtf8(":/markers/images/dragons1.jpg")); showTileGridLines=false; diff --git a/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index f2fcac0fe539c7bc9563240b213ae32516a3b02d..5b351318761dfcaa595e5fe91893d23a20fc2b6f 100644 --- a/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -52,7 +52,15 @@ namespace mapcontrol RefreshToolTip(); RefreshPos(); } - WayPointItem::WayPointItem(const internals::PointLatLng &coord,double const& altitude, const QString &description, MapGraphicItem *map):coord(coord),reached(false),description(description),shownumber(true),isDragging(false),altitude(altitude),heading(0),map(map) + WayPointItem::WayPointItem(const internals::PointLatLng &coord,double const& altitude, const QString &description, MapGraphicItem *map) : + coord(coord), + reached(false), + description(description), + shownumber(true), + isDragging(false), + altitude(altitude), + map(map), + heading(0) { text=0; numberI=0; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dc459edbfb0ca76423bb589c9da8b1054b808933..fe61355ca8668a7056720b2b1fdcff5cbc18f63d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1431,7 +1431,8 @@ void UAS::sendMessage(LinkInterface* link, mavlink_message_t message) uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; // Write message into buffer, prepending start sign int len = mavlink_msg_to_send_buffer(buffer, &message); - mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getId(), message.len); + static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; + mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]); // If link is connected if (link->isConnected()) { diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index 9de38ab4190b8c09845d2c7b6b3713304e5ad68c..13bb1eda026e0993519f31b1d2a9726addbabefb 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -1,5 +1,6 @@ #include +#include "QGCMAVLink.h" #include "QGCMAVLinkInspector.h" #include "ui_QGCMAVLinkInspector.h" @@ -10,63 +11,317 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par ui(new Ui::QGCMAVLinkInspector) { ui->setupUi(this); + + /* Insert system */ + //ui->systemComboBox(); + + mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO; + memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256); + memset(receivedMessages, 0, sizeof(mavlink_message_t)*256); + connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); - ui->treeWidget->setColumnCount(3); + QStringList header; + header << tr("Name"); + header << tr("Value"); + header << tr("Type"); + ui->treeWidget->setHeaderLabels(header); connect(&updateTimer, SIGNAL(timeout()), this, SLOT(refreshView())); updateTimer.start(1000); } void QGCMAVLinkInspector::refreshView() { - foreach (mavlink_message_t msg, receivedMessages.values()) + for (int i = 0; i < 256; ++i)//mavlink_message_t msg, receivedMessages) { + mavlink_message_t* msg = receivedMessages+i; + // Ignore NULL values + if (!msg) continue; // Update the tree view - if (!treeWidgetItems.contains(msg.msgid)) + if (!treeWidgetItems.contains(msg->msgid)) { - QString messageName("MSG (#%1)"); - messageName = messageName.arg(msg.msgid); + QString messageName("%1 (#%2)"); + messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msg->msgid); QStringList fields; fields << messageName; - fields << ""; - fields << ""; QTreeWidgetItem* widget = new QTreeWidgetItem(fields); - treeWidgetItems.insert(msg.msgid, widget); + widget->setFirstColumnSpanned(true); + + for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i) + { + QTreeWidgetItem* field = new QTreeWidgetItem(); + widget->addChild(field); + } + + treeWidgetItems.insert(msg->msgid, widget); ui->treeWidget->addTopLevelItem(widget); } - } -// // List all available MAVLink messages -// QList items; -// for (int i = 0; i < 256; ++i) -// { -// QStringList stringList; -// stringList << QString("message: %1").arg(i) << QString("") << QString("Not received yet."); -// items.append(new QTreeWidgetItem((QTreeWidget*)0, stringList)); -// } -// ui->treeWidget->insertTopLevelItems(0, items); + QTreeWidgetItem* message = treeWidgetItems.value(msg->msgid); + for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i) + { + updateField(msg->msgid, i, message->child(i)); + } + } } void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message) { Q_UNUSED(link); - receivedMessages.insert(message.msgid, message); -// Q_UNUSED(link); -// qDebug() << __FILE__ << __LINE__ << "GOT MAVLINK MESSAGE"; -// // Check if children exist -// const QObjectList& fields = ui->treeWidget->children().at(message.msgid)->children(); - -// if (fields.length() == 0) -// { -// // Create children -// } - -// for (int i = 0; i < fields.length(); ++i) -// { -// // Fill in field data -// } + // Only overwrite if system filter is set +// int filterValue = ui->systemComboBox()->value().toInt(); +// if (filterValue != ) + memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t)); } QGCMAVLinkInspector::~QGCMAVLinkInspector() { delete ui; } + +void QGCMAVLinkInspector::updateField(int msgid, int fieldid, QTreeWidgetItem* item) +{ + // Add field tree widget item + item->setData(0, Qt::DisplayRole, QVariant(messageInfo[msgid].fields[fieldid].name)); + uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8; + switch (messageInfo[msgid].fields[fieldid].type) + { + case MAVLINK_TYPE_CHAR: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + char* str = (char*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + str[messageInfo[msgid].fields[fieldid].array_length-1] = '\0'; + QString string(str); + item->setData(2, Qt::DisplayRole, "char"); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single char + char b = *((char*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, QString("char[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, b); + } + break; + case MAVLINK_TYPE_UINT8_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + uint8_t* nums = m+messageInfo[msgid].fields[fieldid].wire_offset; + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[fieldid].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + uint8_t u = *(m+messageInfo[msgid].fields[fieldid].wire_offset); + item->setData(2, Qt::DisplayRole, "uint8_t"); + item->setData(1, Qt::DisplayRole, u); + } + break; + case MAVLINK_TYPE_INT8_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + int8_t* nums = (int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + int8_t n = *((int8_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "int8_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + case MAVLINK_TYPE_UINT16_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + uint16_t* nums = (uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + uint16_t n = *((uint16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "uint16_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + case MAVLINK_TYPE_INT16_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + int16_t* nums = (int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + int16_t n = *((int16_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "int16_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + case MAVLINK_TYPE_UINT32_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + uint32_t* nums = (uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + float n = *((uint32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "uint32_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + case MAVLINK_TYPE_INT32_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + int32_t* nums = (int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + int32_t n = *((int32_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "int32_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + case MAVLINK_TYPE_FLOAT: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + float* nums = (float*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + float f = *((float*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "float"); + item->setData(1, Qt::DisplayRole, f); + } + break; + case MAVLINK_TYPE_DOUBLE: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + double* nums = (double*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + double f = *((double*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "double"); + item->setData(1, Qt::DisplayRole, f); + } + break; + case MAVLINK_TYPE_UINT64_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + uint64_t* nums = (uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + uint64_t n = *((uint64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "uint64_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + case MAVLINK_TYPE_INT64_T: + if (messageInfo[msgid].fields[fieldid].array_length > 0) + { + int64_t* nums = (int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset); + // Enforce null termination + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < messageInfo[msgid].fields[j].array_length; ++j) + { + string += tmp.arg(nums[j]); + } + item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(messageInfo[msgid].fields[fieldid].array_length)); + item->setData(1, Qt::DisplayRole, string); + } + else + { + // Single value + int64_t n = *((int64_t*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + item->setData(2, Qt::DisplayRole, "int64_t"); + item->setData(1, Qt::DisplayRole, n); + } + break; + } +} diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index e9ba9fb1541e9da9b6e66f81544a3c6477201304..996d74d185ec548894f7f4ab5e8555a18b5cafc9 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/src/ui/QGCMAVLinkInspector.h @@ -27,9 +27,13 @@ public slots: protected: QMap lastFieldUpdate; ///< Used to switch between highlight and non-highlighting color - QMap receivedMessages; ///< Available / known messages + mavlink_message_t receivedMessages[256]; ///< Available / known messages QMap treeWidgetItems; ///< Available tree widget items QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI + mavlink_message_info_t messageInfo[256]; + + // Update one message field + void updateField(int msgid, int fieldid, QTreeWidgetItem* item); private: Ui::QGCMAVLinkInspector *ui; diff --git a/src/ui/QGCMAVLinkInspector.ui b/src/ui/QGCMAVLinkInspector.ui index 0414339eb0e1bcf8799956d475a079e2d9a0ad1d..3d461ac77c34933df970fbdfd66cb79ba2739bc3 100644 --- a/src/ui/QGCMAVLinkInspector.ui +++ b/src/ui/QGCMAVLinkInspector.ui @@ -13,11 +13,11 @@ Form - + 6 - + @@ -26,6 +26,16 @@ + + + + System Filter + + + + + + diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 486fcef255094bbdeeb17d4abc4a1293faaeec33..f7f1b254772318347098474de2e4080908fd684b 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -295,10 +295,9 @@ void QGCParamWidget::addComponent(int uas, int component, QString componentName) components->value(component)->setData(1, Qt::DisplayRole, QString::number(component)); } else { // Add new - QStringList list; - list.append(componentName); - list.append(QString::number(component)); + QStringList list(QString("%1 (#%2)").arg(componentName).arg(component)); QTreeWidgetItem* comp = new QTreeWidgetItem(list); + comp->setFirstColumnSpanned(true); components->insert(component, comp); // Create grouping and update maps paramGroups.insert(component, new QMap()); diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 604ce74b5a0e42cf6bb9079c453df3cc8b5a0ae2..5e03ee940912c8107e56b467b6087170451b00f5 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from ardupilotmega.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 20 2011, 11:19 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -10,44 +9,46 @@ extern "C" { #endif +// MESSAGE LENGTHS AND CRCS -#include "../mavlink_protocol.h" +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51, 5} +#endif -#define MAVLINK_ENABLED_ARDUPILOTMEGA +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {153, 114, 143, 191, 105, 217, 104, 119, 0, 0, 0, 186, 194, 8, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 185, 222, 23, 179, 136, 66, 126, 198, 147, 0, 252, 162, 215, 229, 205, 51, 80, 101, 213, 8, 229, 21, 214, 170, 14, 73, 50, 142, 15, 3, 100, 24, 141, 148, 0, 0, 0, 183, 126, 130, 0, 148, 21, 0, 52, 124, 0, 0, 241, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 250, 156, 0, 0, 0, 0, 0, 0, 0, 146, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 15, 248, 63, 83, 127} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {}, {}, {}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {}, {}, {}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {}, {}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#endif +#include "../protocol.h" + +#define MAVLINK_ENABLED_ARDUPILOTMEGA #include "../common/common.h" + // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif // ENUM DEFINITIONS -// MESSAGE DEFINITIONS +// MESSAGE DEFINITIONS #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" - -// MESSAGE CRC KEYS - -#undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 179, 218, 22, 76, 226, 126, 117, 186, 0, 144, 0, 249, 172, 16, 0, 0, 0, 0, 0, 0, 33, 34, 191, 55, 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, 20, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 80, 0, 127, 200, 0, 0, 212, 251, 20, 38, 22, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 18, 103, 59, 0, 0, 0, 0, 0, 0, 0, 74, 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, 226, 65, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 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, 138, 43, 141, 211, 144 } - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } - #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // ARDUPILOTMEGA_H diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega_testsuite.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega_testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..c75bab6a9cac36c88cd21bcb2cd18a885a2c9a15 --- /dev/null +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega_testsuite.h @@ -0,0 +1,22 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml + * @see http://qgroundcontrol.org/mavlink/ + * Generated on Wed Aug 24 17:14:16 2011 + */ +#ifndef ARDUPILOTMEGA_TESTSUITE_H +#define ARDUPILOTMEGA_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +static void mavtest_generate_outputs(mavlink_channel_t chan) +{ + mavlink_msg_sensor_offsets_send(chan , 19107 , 19211 , 19315 , 17.0 , 963497672 , 963497880 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 , 241.0 ); + mavlink_msg_set_mag_offsets_send(chan , 151 , 218 , 17235 , 17339 , 17443 ); +} + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // ARDUPILOTMEGA_TESTSUITE_H diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 9ff0554ad7a3ea2a8ebb282d6f914faafe521e86..0f83f1ea091fa4d55822d78b94709d83befc0c6b 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,16 +1,23 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol built from ardupilotmega.xml * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, August 20 2011, 11:19 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" +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif -#pragma pack(pop) + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 #endif + +#include "version.h" +#include "ardupilotmega.h" + +#endif // MAVLINK_H diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h index ee8445d85493fb793d0c04d89f2ad42ad8d92c6d..e94e7e8f91db5a2c1981d58822bc3b56a4239b0e 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -1,28 +1,47 @@ // MESSAGE SENSOR_OFFSETS PACKING #define MAVLINK_MSG_ID_SENSOR_OFFSETS 150 -#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42 -#define MAVLINK_MSG_150_LEN 42 -#define MAVLINK_MSG_ID_SENSOR_OFFSETS_KEY 0xE2 -#define MAVLINK_MSG_150_KEY 0xE2 -typedef struct __mavlink_sensor_offsets_t +typedef struct __mavlink_sensor_offsets_t { - float mag_declination; ///< magnetic declination (radians) - int32_t raw_press; ///< raw pressure from barometer - int32_t raw_temp; ///< raw temperature from barometer - float gyro_cal_x; ///< gyro X calibration - float gyro_cal_y; ///< gyro Y calibration - float gyro_cal_z; ///< gyro Z calibration - float accel_cal_x; ///< accel X calibration - float accel_cal_y; ///< accel Y calibration - float accel_cal_z; ///< accel Z calibration - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset - + float mag_declination; ///< magnetic declination (radians) + int32_t raw_press; ///< raw pressure from barometer + int32_t raw_temp; ///< raw temperature from barometer + float gyro_cal_x; ///< gyro X calibration + float gyro_cal_y; ///< gyro Y calibration + float gyro_cal_z; ///< gyro Z calibration + float accel_cal_x; ///< accel X calibration + float accel_cal_y; ///< accel Y calibration + float accel_cal_z; ///< accel Z calibration + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset } mavlink_sensor_offsets_t; +#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42 +#define MAVLINK_MSG_ID_150_LEN 42 + + + +#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \ + "SENSOR_OFFSETS", \ + 12, \ + { { "mag_declination", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \ + { "raw_press", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_sensor_offsets_t, raw_press) }, \ + { "raw_temp", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \ + { "gyro_cal_x", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \ + { "gyro_cal_y", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \ + { "gyro_cal_z", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \ + { "accel_cal_x", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \ + { "accel_cal_y", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \ + { "accel_cal_z", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \ + { "mag_ofs_x", MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \ + { "mag_ofs_y", MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \ + { "mag_ofs_z", MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \ + } \ +} + + /** * @brief Pack a sensor_offsets message * @param system_id ID of this system @@ -43,29 +62,29 @@ typedef struct __mavlink_sensor_offsets_t * @param accel_cal_z accel Z calibration * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - p->mag_ofs_x = mag_ofs_x; // int16_t:magnetometer X offset - p->mag_ofs_y = mag_ofs_y; // int16_t:magnetometer Y offset - p->mag_ofs_z = mag_ofs_z; // int16_t:magnetometer Z offset - p->mag_declination = mag_declination; // float:magnetic declination (radians) - p->raw_press = raw_press; // int32_t:raw pressure from barometer - p->raw_temp = raw_temp; // int32_t:raw temperature from barometer - p->gyro_cal_x = gyro_cal_x; // float:gyro X calibration - p->gyro_cal_y = gyro_cal_y; // float:gyro Y calibration - p->gyro_cal_z = gyro_cal_z; // float:gyro Z calibration - p->accel_cal_x = accel_cal_x; // float:accel X calibration - p->accel_cal_y = accel_cal_y; // float:accel Y calibration - p->accel_cal_z = accel_cal_z; // float:accel Z calibration - - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); + put_float_by_index(msg, 0, mag_declination); // magnetic declination (radians) + put_int32_t_by_index(msg, 4, raw_press); // raw pressure from barometer + put_int32_t_by_index(msg, 8, raw_temp); // raw temperature from barometer + put_float_by_index(msg, 12, gyro_cal_x); // gyro X calibration + put_float_by_index(msg, 16, gyro_cal_y); // gyro Y calibration + put_float_by_index(msg, 20, gyro_cal_z); // gyro Z calibration + put_float_by_index(msg, 24, accel_cal_x); // accel X calibration + put_float_by_index(msg, 28, accel_cal_y); // accel Y calibration + put_float_by_index(msg, 32, accel_cal_z); // accel Z calibration + put_int16_t_by_index(msg, 36, mag_ofs_x); // magnetometer X offset + put_int16_t_by_index(msg, 38, mag_ofs_y); // magnetometer Y offset + put_int16_t_by_index(msg, 40, mag_ofs_z); // magnetometer Z offset + + return mavlink_finalize_message(msg, system_id, component_id, 42, 134); } /** - * @brief Pack a sensor_offsets message + * @brief Pack a sensor_offsets message on a channel * @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 @@ -84,26 +103,70 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_ * @param accel_cal_z accel Z calibration * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) +{ + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; + + put_float_by_index(msg, 0, mag_declination); // magnetic declination (radians) + put_int32_t_by_index(msg, 4, raw_press); // raw pressure from barometer + put_int32_t_by_index(msg, 8, raw_temp); // raw temperature from barometer + put_float_by_index(msg, 12, gyro_cal_x); // gyro X calibration + put_float_by_index(msg, 16, gyro_cal_y); // gyro Y calibration + put_float_by_index(msg, 20, gyro_cal_z); // gyro Z calibration + put_float_by_index(msg, 24, accel_cal_x); // accel X calibration + put_float_by_index(msg, 28, accel_cal_y); // accel Y calibration + put_float_by_index(msg, 32, accel_cal_z); // accel Z calibration + put_int16_t_by_index(msg, 36, mag_ofs_x); // magnetometer X offset + put_int16_t_by_index(msg, 38, mag_ofs_y); // magnetometer Y offset + put_int16_t_by_index(msg, 40, mag_ofs_z); // magnetometer Z offset + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 134); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a sensor_offsets message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + */ +static inline void mavlink_msg_sensor_offsets_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - p->mag_ofs_x = mag_ofs_x; // int16_t:magnetometer X offset - p->mag_ofs_y = mag_ofs_y; // int16_t:magnetometer Y offset - p->mag_ofs_z = mag_ofs_z; // int16_t:magnetometer Z offset - p->mag_declination = mag_declination; // float:magnetic declination (radians) - p->raw_press = raw_press; // int32_t:raw pressure from barometer - p->raw_temp = raw_temp; // int32_t:raw temperature from barometer - p->gyro_cal_x = gyro_cal_x; // float:gyro X calibration - p->gyro_cal_y = gyro_cal_y; // float:gyro Y calibration - p->gyro_cal_z = gyro_cal_z; // float:gyro Z calibration - p->accel_cal_x = accel_cal_x; // float:accel X calibration - p->accel_cal_y = accel_cal_y; // float:accel Y calibration - p->accel_cal_z = accel_cal_z; // float:accel Z calibration - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); + put_float_by_index(msg, 0, mag_declination); // magnetic declination (radians) + put_int32_t_by_index(msg, 4, raw_press); // raw pressure from barometer + put_int32_t_by_index(msg, 8, raw_temp); // raw temperature from barometer + put_float_by_index(msg, 12, gyro_cal_x); // gyro X calibration + put_float_by_index(msg, 16, gyro_cal_y); // gyro Y calibration + put_float_by_index(msg, 20, gyro_cal_z); // gyro Z calibration + put_float_by_index(msg, 24, accel_cal_x); // accel X calibration + put_float_by_index(msg, 28, accel_cal_y); // accel Y calibration + put_float_by_index(msg, 32, accel_cal_z); // accel Z calibration + put_int16_t_by_index(msg, 36, mag_ofs_x); // magnetometer X offset + put_int16_t_by_index(msg, 38, mag_ofs_y); // magnetometer Y offset + put_int16_t_by_index(msg, 40, mag_ofs_z); // magnetometer Z offset + + mavlink_finalize_message_chan_send(msg, chan, 42, 134); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a sensor_offsets struct into a message @@ -118,8 +181,6 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a sensor_offsets message * @param chan MAVLink channel to send the message @@ -137,46 +198,19 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint * @param accel_cal_y accel Y calibration * @param accel_cal_z accel Z calibration */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) { - mavlink_header_t hdr; - mavlink_sensor_offsets_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN ) - payload.mag_ofs_x = mag_ofs_x; // int16_t:magnetometer X offset - payload.mag_ofs_y = mag_ofs_y; // int16_t:magnetometer Y offset - payload.mag_ofs_z = mag_ofs_z; // int16_t:magnetometer Z offset - payload.mag_declination = mag_declination; // float:magnetic declination (radians) - payload.raw_press = raw_press; // int32_t:raw pressure from barometer - payload.raw_temp = raw_temp; // int32_t:raw temperature from barometer - payload.gyro_cal_x = gyro_cal_x; // float:gyro X calibration - payload.gyro_cal_y = gyro_cal_y; // float:gyro Y calibration - payload.gyro_cal_z = gyro_cal_z; // float:gyro Z calibration - payload.accel_cal_x = accel_cal_x; // float:accel X calibration - payload.accel_cal_y = accel_cal_y; // float:accel Y calibration - payload.accel_cal_z = accel_cal_z; // float:accel Z calibration - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN; - hdr.msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xE2, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 42); + mavlink_msg_sensor_offsets_pack_chan_send(chan, msg, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z); } #endif + // MESSAGE SENSOR_OFFSETS UNPACKING + /** * @brief Get field mag_ofs_x from sensor_offsets message * @@ -184,8 +218,7 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16 */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (int16_t)(p->mag_ofs_x); + return MAVLINK_MSG_RETURN_int16_t(msg, 36); } /** @@ -195,8 +228,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_mes */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (int16_t)(p->mag_ofs_y); + return MAVLINK_MSG_RETURN_int16_t(msg, 38); } /** @@ -206,8 +238,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_mes */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (int16_t)(p->mag_ofs_z); + return MAVLINK_MSG_RETURN_int16_t(msg, 40); } /** @@ -217,8 +248,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->mag_declination); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -228,8 +258,7 @@ static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink */ static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (int32_t)(p->raw_press); + return MAVLINK_MSG_RETURN_int32_t(msg, 4); } /** @@ -239,8 +268,7 @@ static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_mes */ static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (int32_t)(p->raw_temp); + return MAVLINK_MSG_RETURN_int32_t(msg, 8); } /** @@ -250,8 +278,7 @@ static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->gyro_cal_x); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -261,8 +288,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->gyro_cal_y); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -272,8 +298,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->gyro_cal_z); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -283,8 +308,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->accel_cal_x); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -294,8 +318,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->accel_cal_y); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -305,8 +328,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) { - mavlink_sensor_offsets_t *p = (mavlink_sensor_offsets_t *)&msg->payload[0]; - return (float)(p->accel_cal_z); + return MAVLINK_MSG_RETURN_float(msg, 32); } /** @@ -317,5 +339,20 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_mes */ static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets) { - memcpy( sensor_offsets, msg->payload, sizeof(mavlink_sensor_offsets_t)); +#if MAVLINK_NEED_BYTE_SWAP + sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg); + sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg); + sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg); + sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg); + sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg); + sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg); + sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg); + sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg); + sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg); + sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg); + sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); + sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); +#else + memcpy(sensor_offsets, MAVLINK_PAYLOAD(msg), 42); +#endif } diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h index d6ff50c0caf06e887e965247865cb93a8d0b7813..3e7086f2042bb7c429e8fdf657e947e23046c14a 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -1,21 +1,33 @@ // MESSAGE SET_MAG_OFFSETS PACKING #define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151 -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8 -#define MAVLINK_MSG_151_LEN 8 -#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_KEY 0x41 -#define MAVLINK_MSG_151_KEY 0x41 -typedef struct __mavlink_set_mag_offsets_t +typedef struct __mavlink_set_mag_offsets_t { - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_set_mag_offsets_t; +#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8 +#define MAVLINK_MSG_ID_151_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \ + "SET_MAG_OFFSETS", \ + 5, \ + { { "mag_ofs_x", MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \ + { "mag_ofs_y", MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \ + { "mag_ofs_z", MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_set_mag_offsets_t, target_component) }, \ + } \ +} + + /** * @brief Pack a set_mag_offsets message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_set_mag_offsets_t * @param mag_ofs_z magnetometer Z offset * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->mag_ofs_x = mag_ofs_x; // int16_t:magnetometer X offset - p->mag_ofs_y = mag_ofs_y; // int16_t:magnetometer Y offset - p->mag_ofs_z = mag_ofs_z; // int16_t:magnetometer Z offset + put_int16_t_by_index(msg, 0, mag_ofs_x); // magnetometer X offset + put_int16_t_by_index(msg, 2, mag_ofs_y); // magnetometer Y offset + put_int16_t_by_index(msg, 4, mag_ofs_z); // magnetometer Z offset + put_uint8_t_by_index(msg, 6, target_system); // System ID + put_uint8_t_by_index(msg, 7, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 8, 219); } /** - * @brief Pack a set_mag_offsets message + * @brief Pack a set_mag_offsets message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8 * @param mag_ofs_z magnetometer Z offset * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->mag_ofs_x = mag_ofs_x; // int16_t:magnetometer X offset - p->mag_ofs_y = mag_ofs_y; // int16_t:magnetometer Y offset - p->mag_ofs_z = mag_ofs_z; // int16_t:magnetometer Z offset + put_int16_t_by_index(msg, 0, mag_ofs_x); // magnetometer X offset + put_int16_t_by_index(msg, 2, mag_ofs_y); // magnetometer Y offset + put_int16_t_by_index(msg, 4, mag_ofs_z); // magnetometer Z offset + put_uint8_t_by_index(msg, 6, target_system); // System ID + put_uint8_t_by_index(msg, 7, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 219); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_mag_offsets message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + */ +static inline void mavlink_msg_set_mag_offsets_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) +{ + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; + + put_int16_t_by_index(msg, 0, mag_ofs_x); // magnetometer X offset + put_int16_t_by_index(msg, 2, mag_ofs_y); // magnetometer Y offset + put_int16_t_by_index(msg, 4, mag_ofs_z); // magnetometer Z offset + put_uint8_t_by_index(msg, 6, target_system); // System ID + put_uint8_t_by_index(msg, 7, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 8, 219); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a set_mag_offsets struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_mag_offsets message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin * @param mag_ofs_y magnetometer Y offset * @param mag_ofs_z magnetometer Z offset */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) { - mavlink_header_t hdr; - mavlink_set_mag_offsets_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.mag_ofs_x = mag_ofs_x; // int16_t:magnetometer X offset - payload.mag_ofs_y = mag_ofs_y; // int16_t:magnetometer Y offset - payload.mag_ofs_z = mag_ofs_z; // int16_t:magnetometer Z offset - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN; - hdr.msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x41, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_set_mag_offsets_pack_chan_send(chan, msg, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z); } #endif + // MESSAGE SET_MAG_OFFSETS UNPACKING + /** * @brief Get field target_system from set_mag_offsets message * @@ -135,8 +155,7 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 6); } /** @@ -146,8 +165,7 @@ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlin */ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 7); } /** @@ -157,8 +175,7 @@ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mav */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; - return (int16_t)(p->mag_ofs_x); + return MAVLINK_MSG_RETURN_int16_t(msg, 0); } /** @@ -168,8 +185,7 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_me */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; - return (int16_t)(p->mag_ofs_y); + return MAVLINK_MSG_RETURN_int16_t(msg, 2); } /** @@ -179,8 +195,7 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_me */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) { - mavlink_set_mag_offsets_t *p = (mavlink_set_mag_offsets_t *)&msg->payload[0]; - return (int16_t)(p->mag_ofs_z); + return MAVLINK_MSG_RETURN_int16_t(msg, 4); } /** @@ -191,5 +206,13 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_me */ static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets) { - memcpy( set_mag_offsets, msg->payload, sizeof(mavlink_set_mag_offsets_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg); + set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg); + set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg); + set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); + set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); +#else + memcpy(set_mag_offsets, MAVLINK_PAYLOAD(msg), 8); +#endif } diff --git a/thirdParty/mavlink/include/ardupilotmega/testsuite.h b/thirdParty/mavlink/include/ardupilotmega/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..fa6ef970a07ed3832bac3d7efc2340af3199a6e4 --- /dev/null +++ b/thirdParty/mavlink/include/ardupilotmega/testsuite.h @@ -0,0 +1,99 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef ARDUPILOTMEGA_TESTSUITE_H +#define ARDUPILOTMEGA_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_common(uint8_t, uint8_t); +static void mavlink_test_ardupilotmega(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + mavlink_test_common(system_id, component_id); + mavlink_test_ardupilotmega(system_id, component_id); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_sensor_offsets_t packet2, packet1 = { + 17.0, + 963497672, + 963497880, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 241.0, + 19107, + 19211, + 19315, + }; + if (sizeof(packet2) != 42) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -// *crcAccum += data; // super simple to test } /** @@ -57,76 +56,12 @@ static inline void crc_init(uint16_t* crcAccum) **/ 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); + while (length--) { + crc_accumulate(*pBuffer++, &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); + return crcTmp; } diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 365d5b617002b5a05da482c31a2cca805b0f417f..be0e642addf8b8b4f1245c21e32cd72c77a9dbcf 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from common.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 20 2011, 14:34 UTC */ #ifndef COMMON_H #define COMMON_H @@ -10,11 +9,32 @@ extern "C" { #endif +// MESSAGE LENGTHS AND CRCS -#include "../mavlink_protocol.h" +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51, 5} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {153, 114, 143, 191, 105, 217, 104, 119, 0, 0, 0, 186, 194, 8, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 185, 222, 23, 179, 136, 66, 126, 198, 147, 0, 252, 162, 215, 229, 205, 51, 80, 101, 213, 8, 229, 21, 214, 170, 14, 73, 50, 142, 15, 3, 100, 24, 141, 148, 0, 0, 0, 183, 126, 130, 0, 148, 21, 0, 52, 124, 0, 0, 241, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 250, 156, 0, 0, 0, 0, 0, 0, 0, 146, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 15, 248, 63, 83, 127} +#endif + +#define MAVLINK_MESSAGE_INFO_EMPTY { \ + "EMPTY", \ + 0, \ + {} \ +} + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_EMPTY, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#endif + +#include "../protocol.h" #define MAVLINK_ENABLED_COMMON + + // MAVLINK VERSION #ifndef MAVLINK_VERSION @@ -28,221 +48,244 @@ extern "C" { // ENUM DEFINITIONS + +/** @brief Valid data types for parameter interface */ +enum MAV_DATA_TYPE +{ + MAV_DATA_TYPE_FLOAT=0, /* IEEE-754 floating point number | */ + MAV_DATA_TYPE_UINT32=1, /* 32 bit unsigned integer (C99: uint32_t) | */ + MAV_DATA_TYPE_INT32=2, /* 32 bit signed integer (C99: int32_t) | */ + MAV_DATA_TYPE_ENUM_END=3, /* | */ +}; + /** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ enum MAV_AUTOPILOT { - MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything */ - MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch */ - MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu */ - MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com */ - MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org */ - MAV_AUTOPILOT_GENERIC_MISSION_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints */ - MAV_AUTOPILOT_GENERIC_MISSION_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands */ - MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set */ - MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component */ - MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi */ - MAV_AUTOPILOT_UDB=10, /* UAV Dev Board */ - MAV_AUTOPILOT_FP=11, /* FlexiPilot */ - MAV_AUTOPILOT_ENUM_END + MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */ + MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ + MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_AUTOPILOT_GENERIC_MISSION_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_AUTOPILOT_GENERIC_MISSION_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ + MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ + MAV_AUTOPILOT_ENUM_END=12, /* | */ }; /** @brief MAV safety override. This is a top-level safety override. Independent of the general state machine this mode shall ensure the safe operation. the MAV_SAFETY state should ONLY be switchable in the MAV_STATE_STANDBY state and NOT in flight. */ enum MAV_SAFETY { - MAV_SAFETY_HIL=0, /* MAV safety lock set to hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. */ - MAV_SAFETY_DISARMED=1, /* MAV safety set to disarmed. Motors are BLOCKED. Independent of the system mode and navigation mode all actuators are blocked. */ - MAV_SAFETY_ARMED=2, /* MAV safety set to armed. Motors are ready / could be started. All actuators are active. */ - MAV_SAFETY_ENUM_END + MAV_SAFETY_HIL=0, /* MAV safety lock set to hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_SAFETY_DISARMED=1, /* MAV safety set to disarmed. Motors are BLOCKED. Independent of the system mode and navigation mode all actuators are blocked. | */ + MAV_SAFETY_ARMED=2, /* MAV safety set to armed. Motors are ready / could be started. All actuators are active. | */ + MAV_SAFETY_ENUM_END=3, /* | */ }; /** @brief */ enum MAV_MODE { - MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. */ - MAV_MODE_STABILIZE=1, /* System is allowed to be active, under assisted RC control. Level of stabilization depends on MAV_FLIGTH_MODE */ - MAV_MODE_MANUAL=2, /* System is allowed to be active, under manual (RC) control, no stabilization */ - 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 (the trajectory is decided onboard and not pre-programmed by waypoints) */ - MAV_MODE_TEST=5, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. */ - MAV_MODE_ENUM_END + MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. | */ + MAV_MODE_STABILIZE=1, /* System is allowed to be active, under assisted RC control. Level of stabilization depends on MAV_FLIGTH_MODE | */ + MAV_MODE_MANUAL=2, /* System is allowed to be active, under manual (RC) control, no stabilization | */ + 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 (the trajectory is decided onboard and not pre-programmed by waypoints) | */ + MAV_MODE_TEST=5, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ + MAV_MODE_ENUM_END=6, /* | */ }; /** @brief */ enum MAV_FLIGHT_MODE { - MAV_FLIGHT_MODE_PREFLIGHT=0, /* System is currently on ground. */ - MAV_FLIGHT_MODE_MANUAL=1, /* No interaction of the autopilot with the actuator outputs, pure manual flight. */ - MAV_FLIGHT_MODE_AUTO_TAKEOFF=2, /* System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. */ - MAV_FLIGHT_MODE_AUTO_HOLD=3, /* System is holding its current position and disabled the mission management. Loitering in mission mode is NOT the hold type, but still mission mode. */ - MAV_FLIGHT_MODE_AUTO_MISSION=4, /* System is navigating towards the next waypoint and following a mission script. */ - MAV_FLIGHT_MODE_AUTO_VECTOR=5, /* System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_FLIGHT_MODE_FBW_CURSOR_CONTROL */ - MAV_FLIGHT_MODE_AUTO_RETURNING=6, /* System is returning straight to home position. Once it reaches there it will hover or loiter at the autopilot's default holding settings. */ - MAV_FLIGHT_MODE_AUTO_LANDING=7, /* System is landing. Throttle is controlled by the autopilot. After getting closer than 30 meters, the course will lock to the current heading. Flare, throttle, flaps, gear, and other events can be scripted based on distance to landing point. */ - MAV_FLIGHT_MODE_AUTO_LOST=8, /* System lost its position input and is in attitude / flight stabilization only. */ - MAV_FLIGHT_MODE_STABILIZE_RATES_ACRO=9, /* The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. */ - MAV_FLIGHT_MODE_STABILIZE_LEVELING=10, /* RC control with stabilization; let go of the sticks and it will level. */ - MAV_FLIGHT_MODE_STABILIZE_ROLL_PITCH_ABSOLUTE=11, /* The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. */ - MAV_FLIGHT_MODE_STABILIZE_ROLL_YAW_ALTITUDE=12, /* Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. */ - MAV_FLIGHT_MODE_STABILIZE_ROLL_PITCH_YAW_ALTITUDE=13, /* Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. */ - MAV_FLIGHT_MODE_STABILIZE_CURSOR_CONTROL=14, /* Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_FLIGHT_MODE_AUTO_VECTOR with user input) */ - MAV_FLIGHT_MODE_TEST1=15, /* Custom test mode, depends on individual autopilot. */ - MAV_FLIGHT_MODE_TEST2=16, /* Custom test mode, depends on individual autopilot. */ - MAV_FLIGHT_MODE_TEST3=17, /* Custom test mode, depends on individual autopilot. */ - MAV_FLIGHT_MODE_ENUM_END + MAV_FLIGHT_MODE_PREFLIGHT=0, /* System is currently on ground. | */ + MAV_FLIGHT_MODE_MANUAL=1, /* No interaction of the autopilot with the actuator outputs, pure manual flight. | */ + MAV_FLIGHT_MODE_AUTO_TAKEOFF=2, /* System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. | */ + MAV_FLIGHT_MODE_AUTO_HOLD=3, /* System is holding its current position and disabled the mission management. Loitering in mission mode is NOT the hold type, but still mission mode. | */ + MAV_FLIGHT_MODE_AUTO_MISSION=4, /* System is navigating towards the next waypoint and following a mission script. | */ + MAV_FLIGHT_MODE_AUTO_VECTOR=5, /* System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_FLIGHT_MODE_FBW_CURSOR_CONTROL | */ + MAV_FLIGHT_MODE_AUTO_RETURNING=6, /* System is returning straight to home position. Once it reaches there it will hover or loiter at the autopilot's default holding settings. | */ + MAV_FLIGHT_MODE_AUTO_LANDING=7, /* System is landing. Throttle is controlled by the autopilot. After getting closer than 30 meters, the course will lock to the current heading. Flare, throttle, flaps, gear, and other events can be scripted based on distance to landing point. | */ + MAV_FLIGHT_MODE_AUTO_LOST=8, /* System lost its position input and is in attitude / flight stabilization only. | */ + MAV_FLIGHT_MODE_STABILIZE_RATES_ACRO=9, /* The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. | */ + MAV_FLIGHT_MODE_STABILIZE_LEVELING=10, /* RC control with stabilization; let go of the sticks and it will level. | */ + MAV_FLIGHT_MODE_STABILIZE_ROLL_PITCH_ABSOLUTE=11, /* The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. | */ + MAV_FLIGHT_MODE_STABILIZE_ROLL_YAW_ALTITUDE=12, /* Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. | */ + MAV_FLIGHT_MODE_STABILIZE_ROLL_PITCH_YAW_ALTITUDE=13, /* Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. | */ + MAV_FLIGHT_MODE_STABILIZE_CURSOR_CONTROL=14, /* Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_FLIGHT_MODE_AUTO_VECTOR with user input) | */ + MAV_FLIGHT_MODE_TEST1=15, /* Custom test mode, depends on individual autopilot. | */ + MAV_FLIGHT_MODE_TEST2=16, /* Custom test mode, depends on individual autopilot. | */ + MAV_FLIGHT_MODE_TEST3=17, /* Custom test mode, depends on individual autopilot. | */ + MAV_FLIGHT_MODE_ENUM_END=18, /* | */ }; /** @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 + 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=8, /* | */ }; /** @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 + 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_ANTENNA_TRACKER=5, /* Ground installation | */ + MAV_TYPE_GCS=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_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_TRICOPTER=15, /* Octorotor | */ + MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + MAV_TYPE_ENUM_END=17, /* | */ }; /** @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 + 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=154, /* | */ }; /** @brief */ enum MAV_FRAME { - MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) */ - 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 with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. */ - MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) */ - MAV_FRAME_ENUM_END + MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */ + 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 with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ + MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ + MAV_FRAME_ENUM_END=5, /* | */ }; /** @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 + 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=6, /* | */ }; -/** @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. */ +/** @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 + 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=246, /* | */ }; -/** @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_ENUM_END + 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=13, /* | */ }; -/** @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_NONE=0, /* No region of interest. */ - MAV_ROI_WPNEXT=1, /* Point toward next waypoint. */ - MAV_ROI_WPINDEX=2, /* Point toward given waypoint. */ - MAV_ROI_LOCATION=3, /* Point toward fixed location. */ - MAV_ROI_TARGET=4, /* Point toward of given id. */ - MAV_ROI_ENUM_END + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ + MAV_ROI_ENUM_END=5, /* | */ }; - // MESSAGE DEFINITIONS - #include "./mavlink_msg_heartbeat.h" #include "./mavlink_msg_sys_status.h" #include "./mavlink_msg_system_time.h" @@ -314,18 +357,7 @@ enum MAV_ROI #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" - -// MESSAGE CRC KEYS - -#undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 10, 218, 22, 76, 226, 126, 117, 186, 0, 0, 0, 249, 172, 16, 0, 0, 0, 0, 0, 0, 33, 34, 191, 55, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 0, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 20, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 0, 42, 80, 0, 127, 200, 0, 0, 212, 251, 20, 63, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 103, 59, 0, 0, 0, 0, 0, 0, 0, 74, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 } - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } - #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // COMMON_H diff --git a/thirdParty/mavlink/include/common/common_testsuite.h b/thirdParty/mavlink/include/common/common_testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..ddeeef3757d563e82fa9cd69577f472e8e9f758b --- /dev/null +++ b/thirdParty/mavlink/include/common/common_testsuite.h @@ -0,0 +1,90 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from common.xml + * @see http://qgroundcontrol.org/mavlink/ + * Generated on Wed Aug 24 17:14:16 2011 + */ +#ifndef COMMON_TESTSUITE_H +#define COMMON_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +static void mavtest_generate_outputs(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send(chan , 5 , 72 , 139 , 206 , 17 , 84 , 151 ); + mavlink_msg_sys_status_send(chan , 17235 , 17339 , 17443 , 17547 , 17651 , 17755 , 17859 , 199 , 17963 , 18067 , 18171 , 18275 ); + mavlink_msg_system_time_send(chan , 9223372036854775807 , 963497880 ); + mavlink_msg_system_time_utc_send(chan , 963497464 , 963497672 ); + mavlink_msg_ping_send(chan , 963497880 , 41 , 108 , 9223372036854775807 ); + mavlink_msg_change_operator_control_send(chan , 5 , 72 , 139 , "AQGWMCSIYOEUKAQGWMCSIYOEU" ); + mavlink_msg_change_operator_control_ack_send(chan , 5 , 72 , 139 ); + mavlink_msg_auth_key_send(chan , "ARIZQHYPGXOFWNEVMDULCTKBSJARIZQH" ); + mavlink_msg_set_mode_send(chan , 5 , 72 ); + mavlink_msg_set_flight_mode_send(chan , 5 , 72 ); + mavlink_msg_set_safety_mode_send(chan , 5 , 72 ); + mavlink_msg_param_request_read_send(chan , 139 , 206 , "AHOVCJQXELSZGNUB" , 17235 ); + mavlink_msg_param_request_list_send(chan , 5 , 72 ); + mavlink_msg_param_value_send(chan , "AXUROLIFCZWTQNKH" , 17.0 , 77 , 17443 , 17547 ); + mavlink_msg_param_set_send(chan , 17 , 84 , "APETIXMBQFUJYNCR" , 17.0 , 199 ); + mavlink_msg_gps_raw_int_send(chan , 9223372036854775807 , 89 , 963497880 , 963498088 , 963498296 , 18275 , 18379 , 18483 , 18587 , 156 ); + mavlink_msg_scaled_imu_send(chan , 9223372036854775807 , 17651 , 17755 , 17859 , 17963 , 18067 , 18171 , 18275 , 18379 , 18483 ); + mavlink_msg_gps_status_send(chan , 5 , "72" , "132" , "192" , "252" , "56" ); + mavlink_msg_raw_imu_send(chan , 9223372036854775807 , 17651 , 17755 , 17859 , 17963 , 18067 , 18171 , 18275 , 18379 , 18483 ); + mavlink_msg_raw_pressure_send(chan , 9223372036854775807 , 17651 , 17755 , 17859 , 17963 ); + mavlink_msg_scaled_pressure_send(chan , 9223372036854775807 , 73.0 , 101.0 , 18067 ); + mavlink_msg_attitude_send(chan , 9223372036854775807 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 ); + mavlink_msg_local_position_send(chan , 9223372036854775807 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 ); + mavlink_msg_global_position_send(chan , 9223372036854775807 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 ); + mavlink_msg_gps_raw_send(chan , 9223372036854775807 , 113 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 , 241.0 , 180 ); + mavlink_msg_rc_channels_raw_send(chan , 17235 , 17339 , 17443 , 17547 , 17651 , 17755 , 17859 , 17963 , 53 ); + mavlink_msg_rc_channels_scaled_send(chan , 17235 , 17339 , 17443 , 17547 , 17651 , 17755 , 17859 , 17963 , 53 ); + mavlink_msg_servo_output_raw_send(chan , 17235 , 17339 , 17443 , 17547 , 17651 , 17755 , 17859 , 17963 ); + mavlink_msg_waypoint_send(chan , 223 , 34 , 18691 , 101 , 168 , 235 , 46 , 17.0 , 45.0 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 ); + mavlink_msg_waypoint_request_send(chan , 139 , 206 , 17235 ); + mavlink_msg_waypoint_set_current_send(chan , 139 , 206 , 17235 ); + mavlink_msg_waypoint_current_send(chan , 17235 ); + mavlink_msg_waypoint_request_list_send(chan , 5 , 72 ); + mavlink_msg_waypoint_count_send(chan , 139 , 206 , 17235 ); + mavlink_msg_waypoint_clear_all_send(chan , 5 , 72 ); + mavlink_msg_waypoint_reached_send(chan , 17235 ); + mavlink_msg_waypoint_ack_send(chan , 5 , 72 , 139 ); + mavlink_msg_gps_set_global_origin_send(chan , 41 , 108 , 963497464 , 963497672 , 963497880 ); + mavlink_msg_gps_local_origin_set_send(chan , 963497464 , 963497672 , 963497880 ); + mavlink_msg_local_position_setpoint_set_send(chan , 53 , 120 , 17.0 , 45.0 , 73.0 , 101.0 ); + mavlink_msg_local_position_setpoint_send(chan , 17.0 , 45.0 , 73.0 , 101.0 ); + mavlink_msg_global_position_setpoint_int_send(chan , 963497464 , 963497672 , 963497880 , 17859 ); + mavlink_msg_safety_set_allowed_area_send(chan , 77 , 144 , 211 , 17.0 , 45.0 , 73.0 , 101.0 , 129.0 , 157.0 ); + mavlink_msg_safety_allowed_area_send(chan , 77 , 17.0 , 45.0 , 73.0 , 101.0 , 129.0 , 157.0 ); + mavlink_msg_set_roll_pitch_yaw_thrust_send(chan , 53 , 120 , 17.0 , 45.0 , 73.0 , 101.0 ); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(chan , 53 , 120 , 17.0 , 45.0 , 73.0 , 101.0 ); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(chan , 963497464 , 45.0 , 73.0 , 101.0 , 129.0 ); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(chan , 963497464 , 45.0 , 73.0 , 101.0 , 129.0 ); + mavlink_msg_nav_controller_output_send(chan , 17.0 , 45.0 , 18275 , 18379 , 18483 , 73.0 , 101.0 , 129.0 ); + mavlink_msg_position_target_send(chan , 17.0 , 45.0 , 73.0 , 101.0 ); + mavlink_msg_state_correction_send(chan , 17.0 , 45.0 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 , 241.0 ); + mavlink_msg_request_data_stream_send(chan , 139 , 206 , 17 , 17235 , 84 ); + mavlink_msg_data_stream_send(chan , 139 , 17235 , 206 ); + mavlink_msg_manual_control_send(chan , 53 , 17.0 , 45.0 , 73.0 , 101.0 , 120 , 187 , 254 , 65 ); + mavlink_msg_rc_channels_override_send(chan , 53 , 120 , 17235 , 17339 , 17443 , 17547 , 17651 , 17755 , 17859 , 17963 ); + mavlink_msg_global_position_int_send(chan , 963497464 , 963497672 , 963497880 , 17859 , 17963 , 18067 , 18171 ); + mavlink_msg_vfr_hud_send(chan , 17.0 , 45.0 , 18067 , 18171 , 73.0 , 101.0 ); + mavlink_msg_command_short_send(chan , 53 , 120 , 187 , 254 , 17.0 , 45.0 , 73.0 , 101.0 ); + mavlink_msg_command_long_send(chan , 89 , 156 , 223 , 34 , 17.0 , 45.0 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 ); + mavlink_msg_command_ack_send(chan , 17.0 , 45.0 ); + mavlink_msg_hil_state_send(chan , 9223372036854775807 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 , 963499128 , 963499336 , 963499544 , 19523 , 19627 , 19731 , 19835 , 19939 , 20043 ); + mavlink_msg_hil_controls_send(chan , 9223372036854775807 , 73.0 , 101.0 , 129.0 , 157.0 , 185.0 , 213.0 , 241.0 , 269.0 , 125 , 192 ); + mavlink_msg_hil_rc_inputs_raw_send(chan , 9223372036854775807 , 17651 , 17755 , 17859 , 17963 , 18067 , 18171 , 18275 , 18379 , 18483 , 18587 , 18691 , 18795 , 101 ); + mavlink_msg_optical_flow_send(chan , 9223372036854775807 , 53 , 17859 , 17963 , 120 , 73.0 ); + mavlink_msg_memory_vect_send(chan , 17235 , 139 , 206 , "17" ); + mavlink_msg_debug_vect_send(chan , "ATMFYRKDWP" , 9223372036854775807 , 73.0 , 101.0 , 129.0 ); + mavlink_msg_named_value_float_send(chan , "AHOVCJQXEL" , 17.0 ); + mavlink_msg_named_value_int_send(chan , "AHOVCJQXEL" , 963497464 ); + mavlink_msg_statustext_send(chan , 5 , "AIQYGOWEMUCKSAIQYGOWEMUCKSAIQYGOWEMUCKSAIQYGOWEMUC" ); + mavlink_msg_debug_send(chan , 17 , 17.0 ); +} + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // COMMON_TESTSUITE_H diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 90177c292442b9f4960b1953f9a8654a611141e8..2846cb0f007610bd2f02e0187a42bedb1cb57978 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,16 +1,24 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol built from common.xml * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, August 20 2011, 14:34 UTC + * Generated on Fri Aug 26 14:04:08 2011 */ #ifndef MAVLINK_H #define MAVLINK_H -#pragma pack(push,1) -#include "mavlink_options.h" -#include "common.h" -#ifdef MAVLINK_DATA -#include "mavlink_data.h" +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif -#pragma pack(pop) + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 #endif + +#include "version.h" +#include "common.h" + +#endif // MAVLINK_H diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index e1ffc979c3d7710217505b2742b424b6ce0385b8..28a1f83cfe4fc532af879170b18c90aab4c6a426 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -1,23 +1,37 @@ // MESSAGE ATTITUDE PACKING #define MAVLINK_MSG_ID_ATTITUDE 30 -#define MAVLINK_MSG_ID_ATTITUDE_LEN 32 -#define MAVLINK_MSG_30_LEN 32 -#define MAVLINK_MSG_ID_ATTITUDE_KEY 0xF3 -#define MAVLINK_MSG_30_KEY 0xF3 -typedef struct __mavlink_attitude_t +typedef struct __mavlink_attitude_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) - + 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) } mavlink_attitude_t; +#define MAVLINK_MSG_ID_ATTITUDE_LEN 32 +#define MAVLINK_MSG_ID_30_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_ATTITUDE { \ + "ATTITUDE", \ + 7, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_attitude_t, usec) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, yaw) }, \ + { "rollspeed", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, rollspeed) }, \ + { "pitchspeed", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, pitchspeed) }, \ + { "yawspeed", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_t, yawspeed) }, \ + } \ +} + + /** * @brief Pack a attitude message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_attitude_t * @param yawspeed Yaw angular speed (rad/s) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll); // Roll angle (rad) + put_float_by_index(msg, 12, pitch); // Pitch angle (rad) + put_float_by_index(msg, 16, yaw); // Yaw angle (rad) + put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) + put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) + put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 66); } /** - * @brief Pack a attitude message + * @brief Pack a attitude message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp * @param yawspeed Yaw angular speed (rad/s) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll); // Roll angle (rad) + put_float_by_index(msg, 12, pitch); // Pitch angle (rad) + put_float_by_index(msg, 16, yaw); // Yaw angle (rad) + put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) + put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) + put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 66); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a attitude message on a channel and send + * @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) + */ +static inline void mavlink_msg_attitude_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed) +{ + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll); // Roll angle (rad) + put_float_by_index(msg, 12, pitch); // Pitch angle (rad) + put_float_by_index(msg, 16, yaw); // Yaw angle (rad) + put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) + put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) + put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) + + mavlink_finalize_message_chan_send(msg, chan, 32, 66); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a attitude struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->usec, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a attitude message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_attitude_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ATTITUDE_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.roll = roll; // float:Roll angle (rad) - payload.pitch = pitch; // float:Pitch angle (rad) - payload.yaw = yaw; // float:Yaw angle (rad) - payload.rollspeed = rollspeed; // float:Roll angular speed (rad/s) - payload.pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF3, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_attitude_pack_chan_send(chan, msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); } #endif + // MESSAGE ATTITUDE UNPACKING + /** * @brief Get field usec from attitude message * @@ -149,8 +173,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (float)(p->rollspeed); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (float)(p->pitchspeed); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; - return (float)(p->yawspeed); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( attitude, msg->payload, sizeof(mavlink_attitude_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(attitude, MAVLINK_PAYLOAD(msg), 32); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index b98e0c969e0a29c08c29d9a5faa684f18f471b3c..ca24dd88f693092d250e9bfb9a78153666c7b231 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -1,18 +1,25 @@ // 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 -#define MAVLINK_MSG_ID_AUTH_KEY_KEY 0xBA -#define MAVLINK_MSG_7_KEY 0xBA -typedef struct __mavlink_auth_key_t +typedef struct __mavlink_auth_key_t { - char key[32]; ///< key - + char key[32]; ///< key } mavlink_auth_key_t; + +#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 +#define MAVLINK_MSG_ID_7_LEN 32 + #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 +#define MAVLINK_MESSAGE_INFO_AUTH_KEY { \ + "AUTH_KEY", \ + 1, \ + { { "key", MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ + } \ +} + + /** * @brief Pack a auth_key message * @param system_id ID of this system @@ -22,18 +29,18 @@ typedef struct __mavlink_auth_key_t * @param key key * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* key) +static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *key) { - mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - memcpy(p->key, key, sizeof(p->key)); // char[32]:key + put_char_array_by_index(msg, 0, key, 32); // key - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 119); } /** - * @brief Pack a auth_key message + * @brief Pack a auth_key message on a channel * @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 @@ -41,16 +48,38 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp * @param key key * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - memcpy(p->key, key, sizeof(p->key)); // char[32]:key + put_char_array_by_index(msg, 0, key, 32); // key - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 119); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a auth_key message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param key key + */ +static inline void mavlink_msg_auth_key_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + const char *key) +{ + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; + + put_char_array_by_index(msg, 0, key, 32); // key + + mavlink_finalize_message_chan_send(msg, chan, 32, 119); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a auth_key struct into a message * @@ -64,54 +93,33 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a auth_key message * @param chan MAVLink channel to send the message * * @param key key */ -static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) { - mavlink_header_t hdr; - mavlink_auth_key_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_AUTH_KEY_LEN ) - memcpy(payload.key, key, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xBA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_auth_key_pack_chan_send(chan, msg, key); } #endif + // MESSAGE AUTH_KEY UNPACKING + /** * @brief Get field key from auth_key message * * @return key */ -static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char* key) +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(key, p->key, sizeof(p->key)); - return sizeof(p->key); + return MAVLINK_MSG_RETURN_char_array(msg, key, 32, 0); } /** @@ -122,5 +130,9 @@ 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) { - memcpy( auth_key, msg->payload, sizeof(mavlink_auth_key_t)); +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_auth_key_get_key(msg, auth_key->key); +#else + memcpy(auth_key, MAVLINK_PAYLOAD(msg), 32); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 8b67edb82c788ee76ffa3d434e5ee04b6eb1e368..c19eb67582e36fd43584bbeb30da824c9248eab9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -1,15 +1,10 @@ // MESSAGE BOOT PACKING #define MAVLINK_MSG_ID_BOOT 1 -#define MAVLINK_MSG_ID_BOOT_LEN 4 -#define MAVLINK_MSG_1_LEN 4 -#define MAVLINK_MSG_ID_BOOT_KEY 0xF9 -#define MAVLINK_MSG_1_KEY 0xF9 -typedef struct __mavlink_boot_t +typedef struct __mavlink_boot_t { - uint32_t version; ///< The onboard software version - + uint32_t version; ///< The onboard software version } mavlink_boot_t; /** @@ -21,18 +16,18 @@ typedef struct __mavlink_boot_t * @param version The onboard software version * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t version) +static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t version) { - mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - p->version = version; // uint32_t:The onboard software version + put_uint32_t_by_index(version, 0, msg->payload); // The onboard software version - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BOOT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 61); } /** - * @brief Pack a boot message + * @brief Pack a boot message on a channel * @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 @@ -40,16 +35,38 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen * @param version The onboard software version * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - p->version = version; // uint32_t:The onboard software version + put_uint32_t_by_index(version, 0, msg->payload); // The onboard software version - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BOOT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 61); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a boot message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param version The onboard software version + */ +static inline void mavlink_msg_boot_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint32_t version) +{ + msg->msgid = MAVLINK_MSG_ID_BOOT; + + put_uint32_t_by_index(version, 0, msg->payload); // The onboard software version + + mavlink_finalize_message_chan_send(msg, chan, 4, 61); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a boot struct into a message * @@ -63,43 +80,25 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon return mavlink_msg_boot_pack(system_id, component_id, msg, boot->version); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a boot message * @param chan MAVLink channel to send the message * * @param version The onboard software version */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { - mavlink_header_t hdr; - mavlink_boot_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_BOOT_LEN ) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_boot_pack_chan_send(chan, msg, version); } #endif + // MESSAGE BOOT UNPACKING + /** * @brief Get field version from boot message * @@ -107,8 +106,7 @@ 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) { - mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; - return (uint32_t)(p->version); + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); } /** @@ -119,5 +117,9 @@ 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) { - memcpy( boot, msg->payload, sizeof(mavlink_boot_t)); +#if MAVLINK_NEED_BYTE_SWAP + boot->version = mavlink_msg_boot_get_version(msg); +#else + memcpy(boot, msg->payload, 4); +#endif } 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 029fb1817b10d7c3cb643846153527b443f47cf9..47c919f61b8347b1c2d272730e13369330933a2b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -1,21 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_KEY 0x7E -#define MAVLINK_MSG_5_KEY 0x7E -typedef struct __mavlink_change_operator_control_t +typedef struct __mavlink_change_operator_control_t { - uint8_t target_system; ///< System the GCS requests control for - uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - uint8_t 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. - 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 "!?,.-" - + uint8_t target_system; ///< System the GCS requests control for + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t 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. + 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_ID_CHANGE_OPERATOR_CONTROL_LEN 28 +#define MAVLINK_MSG_ID_5_LEN 28 + #define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 +#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL { \ + "CHANGE_OPERATOR_CONTROL", \ + 4, \ + { { "target_system", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \ + { "control_request", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \ + { "version", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \ + { "passkey", MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \ + } \ +} + + /** * @brief Pack a change_operator_control message * @param system_id ID of this system @@ -28,21 +38,21 @@ typedef struct __mavlink_change_operator_control_t * @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 "!?,.-" * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - 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 "!?,.-" + put_uint8_t_by_index(msg, 0, target_system); // System the GCS requests control for + put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV + put_uint8_t_by_index(msg, 2, 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. + put_char_array_by_index(msg, 3, 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 "!?,.-" - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 28, 217); } /** - * @brief Pack a change_operator_control message + * @brief Pack a change_operator_control message on a channel * @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 @@ -53,18 +63,46 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i * @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 "!?,.-" * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + + put_uint8_t_by_index(msg, 0, target_system); // System the GCS requests control for + put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV + put_uint8_t_by_index(msg, 2, 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. + put_char_array_by_index(msg, 3, 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 "!?,.-" + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 217); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a change_operator_control message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @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 "!?,.-" + */ +static inline void mavlink_msg_change_operator_control_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey) { - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - 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 "!?,.-" + put_uint8_t_by_index(msg, 0, target_system); // System the GCS requests control for + put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV + put_uint8_t_by_index(msg, 2, 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. + put_char_array_by_index(msg, 3, 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 "!?,.-" - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); + mavlink_finalize_message_chan_send(msg, chan, 28, 217); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a change_operator_control struct into a message @@ -79,8 +117,6 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a change_operator_control message * @param chan MAVLink channel to send the message @@ -90,38 +126,19 @@ 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 "!?,.-" */ -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) +#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_header_t hdr; - mavlink_change_operator_control_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN ) - payload.target_system = target_system; // uint8_t:System the GCS requests control for - payload.control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - payload.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(payload.passkey, passkey, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x7E, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 28); + mavlink_msg_change_operator_control_pack_chan_send(chan, msg, target_system, control_request, version, passkey); } #endif + // MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING + /** * @brief Get field target_system from change_operator_control message * @@ -129,8 +146,7 @@ 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) { - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -140,8 +156,7 @@ 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) { - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; - return (uint8_t)(p->control_request); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -151,8 +166,7 @@ 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) { - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; - return (uint8_t)(p->version); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -160,12 +174,9 @@ 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* passkey) +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(passkey, p->passkey, sizeof(p->passkey)); - return sizeof(p->passkey); + return MAVLINK_MSG_RETURN_char_array(msg, passkey, 25, 3); } /** @@ -176,5 +187,12 @@ 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) { - memcpy( change_operator_control, msg->payload, sizeof(mavlink_change_operator_control_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(change_operator_control, MAVLINK_PAYLOAD(msg), 28); +#endif } 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 4df70cc662665683d4f7771c8bc86517589013ff..6cc9ef58a3a7949b9db2d35f73cb50497ea870e0 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,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_KEY 0x75 -#define MAVLINK_MSG_6_KEY 0x75 -typedef struct __mavlink_change_operator_control_ack_t +typedef struct __mavlink_change_operator_control_ack_t { - uint8_t gcs_system_id; ///< ID of the GCS this message - uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - + uint8_t gcs_system_id; ///< ID of the GCS this message + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control } mavlink_change_operator_control_ack_t; +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 +#define MAVLINK_MSG_ID_6_LEN 3 + + + +#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK { \ + "CHANGE_OPERATOR_CONTROL_ACK", \ + 3, \ + { { "gcs_system_id", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_ack_t, gcs_system_id) }, \ + { "control_request", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_ack_t, control_request) }, \ + { "ack", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_ack_t, ack) }, \ + } \ +} + + /** * @brief Pack a change_operator_control_ack message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_change_operator_control_ack_t * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_uint8_t_by_index(msg, 0, gcs_system_id); // ID of the GCS this message + put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV + put_uint8_t_by_index(msg, 2, ack); // 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, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 3, 104); } /** - * @brief Pack a change_operator_control_ack message + * @brief Pack a change_operator_control_ack message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_uint8_t_by_index(msg, 0, gcs_system_id); // ID of the GCS this message + put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV + put_uint8_t_by_index(msg, 2, ack); // 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, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a change_operator_control_ack message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param gcs_system_id ID of the GCS this message + * @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 + */ +static inline void mavlink_msg_change_operator_control_ack_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t gcs_system_id,uint8_t control_request,uint8_t ack) +{ + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + + put_uint8_t_by_index(msg, 0, gcs_system_id); // ID of the GCS this message + put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV + put_uint8_t_by_index(msg, 2, ack); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + mavlink_finalize_message_chan_send(msg, chan, 3, 104); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a change_operator_control_ack struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a change_operator_control_ack message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + 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_header_t hdr; - mavlink_change_operator_control_ack_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN ) - payload.gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message - payload.control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x75, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 3); + mavlink_msg_change_operator_control_ack_pack_chan_send(chan, msg, gcs_system_id, control_request, ack); } #endif + // MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING + /** * @brief Get field gcs_system_id from change_operator_control_ack message * @@ -121,8 +137,7 @@ 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) { - mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; - return (uint8_t)(p->gcs_system_id); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; - return (uint8_t)(p->control_request); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; - return (uint8_t)(p->ack); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( change_operator_control_ack, msg->payload, sizeof(mavlink_change_operator_control_ack_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(change_operator_control_ack, MAVLINK_PAYLOAD(msg), 3); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index 1fd103f947ef3c97725bd8a0a524d2305b7cc180..d2547367fdf9eee4f732fd6640de1cfa249a2754 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -1,18 +1,27 @@ // MESSAGE COMMAND_ACK PACKING #define MAVLINK_MSG_ID_COMMAND_ACK 77 -#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 -#define MAVLINK_MSG_77_LEN 8 -#define MAVLINK_MSG_ID_COMMAND_ACK_KEY 0x16 -#define MAVLINK_MSG_77_KEY 0x16 -typedef struct __mavlink_command_ack_t +typedef struct __mavlink_command_ack_t { - float command; ///< Current airspeed in m/s - float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - + float command; ///< Current airspeed in m/s + float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION } mavlink_command_ack_t; +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 +#define MAVLINK_MSG_ID_77_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ + "COMMAND_ACK", \ + 2, \ + { { "command", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ + { "result", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_ack_t, result) }, \ + } \ +} + + /** * @brief Pack a command_ack message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_command_ack_t * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - 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 + put_float_by_index(msg, 0, command); // Current airspeed in m/s + put_float_by_index(msg, 4, result); // 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, MAVLINK_MSG_ID_COMMAND_ACK_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 8, 8); } /** - * @brief Pack a command_ack message + * @brief Pack a command_ack message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; + + put_float_by_index(msg, 0, command); // Current airspeed in m/s + put_float_by_index(msg, 4, result); // 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, 8, 8); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a command_ack message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @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 + */ +static inline void mavlink_msg_command_ack_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float command,float result) { - mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - 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 + put_float_by_index(msg, 0, command); // Current airspeed in m/s + put_float_by_index(msg, 4, result); // 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, MAVLINK_MSG_ID_COMMAND_ACK_LEN); + mavlink_finalize_message_chan_send(msg, chan, 8, 8); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a command_ack struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a command_ack message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { - mavlink_header_t hdr; - mavlink_command_ack_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN ) - payload.command = command; // float:Current airspeed in m/s - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x16, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_command_ack_pack_chan_send(chan, msg, command, result); } #endif + // MESSAGE COMMAND_ACK UNPACKING + /** * @brief Get field command from command_ack message * @@ -114,8 +128,7 @@ 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) { - mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; - return (float)(p->command); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; - return (float)(p->result); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( command_ack, msg->payload, sizeof(mavlink_command_ack_t)); +#if MAVLINK_NEED_BYTE_SWAP + command_ack->command = mavlink_msg_command_ack_get_command(msg); + command_ack->result = mavlink_msg_command_ack_get_result(msg); +#else + memcpy(command_ack, MAVLINK_PAYLOAD(msg), 8); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_long.h b/thirdParty/mavlink/include/common/mavlink_msg_command_long.h index 535424bd4327ea6097c782425dee808d98fd7165..0c498e16558b815e5afe6487faf52a5b39fe247f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_long.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_long.h @@ -1,27 +1,45 @@ // MESSAGE COMMAND_LONG PACKING #define MAVLINK_MSG_ID_COMMAND_LONG 76 -#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 32 -#define MAVLINK_MSG_76_LEN 32 -#define MAVLINK_MSG_ID_COMMAND_LONG_KEY 0x3F -#define MAVLINK_MSG_76_KEY 0x3F -typedef struct __mavlink_command_long_t +typedef struct __mavlink_command_long_t { - 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. - float param5; ///< Parameter 5, as defined by MAV_CMD enum. - float param6; ///< Parameter 6, as defined by MAV_CMD enum. - float param7; ///< Parameter 7, 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) - + 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. + float param5; ///< Parameter 5, as defined by MAV_CMD enum. + float param6; ///< Parameter 6, as defined by MAV_CMD enum. + float param7; ///< Parameter 7, 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_long_t; +#define MAVLINK_MSG_ID_COMMAND_LONG_LEN 32 +#define MAVLINK_MSG_ID_76_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_LONG { \ + "COMMAND_LONG", \ + 11, \ + { { "param1", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_long_t, param1) }, \ + { "param2", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_long_t, param2) }, \ + { "param3", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_long_t, param3) }, \ + { "param4", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_long_t, param4) }, \ + { "param5", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_long_t, param5) }, \ + { "param6", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_command_long_t, param6) }, \ + { "param7", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_command_long_t, param7) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_command_long_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_command_long_t, target_component) }, \ + { "command", MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_command_long_t, command) }, \ + { "confirmation", MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_command_long_t, confirmation) }, \ + } \ +} + + /** * @brief Pack a command_long message * @param system_id ID of this system @@ -41,28 +59,28 @@ typedef struct __mavlink_command_long_t * @param param7 Parameter 7, as defined by MAV_CMD enum. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_command_long_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, float param5, float param6, float param7) +static inline uint16_t mavlink_msg_command_long_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, float param5, float param6, float param7) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - 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. - p->param5 = param5; // float:Parameter 5, as defined by MAV_CMD enum. - p->param6 = param6; // float:Parameter 6, as defined by MAV_CMD enum. - p->param7 = param7; // float:Parameter 7, as defined by MAV_CMD enum. - - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_LEN); + put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. + put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. + put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. + put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. + put_float_by_index(msg, 16, param5); // Parameter 5, as defined by MAV_CMD enum. + put_float_by_index(msg, 20, param6); // Parameter 6, as defined by MAV_CMD enum. + put_float_by_index(msg, 24, param7); // Parameter 7, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 28, target_system); // System which should execute the command + put_uint8_t_by_index(msg, 29, target_component); // Component which should execute the command, 0 for all components + put_uint8_t_by_index(msg, 30, command); // Command ID, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 31, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + + return mavlink_finalize_message(msg, system_id, component_id, 32, 168); } /** - * @brief Pack a command_long message + * @brief Pack a command_long message on a channel * @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 @@ -80,26 +98,68 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t * @param param7 Parameter 7, as defined by MAV_CMD enum. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_command_long_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, float param5, float param6, float param7) +static inline uint16_t mavlink_msg_command_long_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,float param5,float param6,float param7) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - 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. - p->param5 = param5; // float:Parameter 5, as defined by MAV_CMD enum. - p->param6 = param6; // float:Parameter 6, as defined by MAV_CMD enum. - p->param7 = param7; // float:Parameter 7, as defined by MAV_CMD enum. - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN); + put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. + put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. + put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. + put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. + put_float_by_index(msg, 16, param5); // Parameter 5, as defined by MAV_CMD enum. + put_float_by_index(msg, 20, param6); // Parameter 6, as defined by MAV_CMD enum. + put_float_by_index(msg, 24, param7); // Parameter 7, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 28, target_system); // System which should execute the command + put_uint8_t_by_index(msg, 29, target_component); // Component which should execute the command, 0 for all components + put_uint8_t_by_index(msg, 30, command); // Command ID, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 31, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 168); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a command_long message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System which should execute the command + * @param target_component Component which should execute the command, 0 for all components + * @param command Command ID, as defined by MAV_CMD enum. + * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + * @param param1 Parameter 1, as defined by MAV_CMD enum. + * @param param2 Parameter 2, as defined by MAV_CMD enum. + * @param param3 Parameter 3, as defined by MAV_CMD enum. + * @param param4 Parameter 4, as defined by MAV_CMD enum. + * @param param5 Parameter 5, as defined by MAV_CMD enum. + * @param param6 Parameter 6, as defined by MAV_CMD enum. + * @param param7 Parameter 7, as defined by MAV_CMD enum. + */ +static inline void mavlink_msg_command_long_pack_chan_send(mavlink_channel_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,float param5,float param6,float param7) +{ + msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; + + put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. + put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. + put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. + put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. + put_float_by_index(msg, 16, param5); // Parameter 5, as defined by MAV_CMD enum. + put_float_by_index(msg, 20, param6); // Parameter 6, as defined by MAV_CMD enum. + put_float_by_index(msg, 24, param7); // Parameter 7, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 28, target_system); // System which should execute the command + put_uint8_t_by_index(msg, 29, target_component); // Component which should execute the command, 0 for all components + put_uint8_t_by_index(msg, 30, command); // Command ID, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 31, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + + mavlink_finalize_message_chan_send(msg, chan, 32, 168); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a command_long struct into a message * @@ -113,8 +173,6 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ return mavlink_msg_command_long_pack(system_id, component_id, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a command_long message * @param chan MAVLink channel to send the message @@ -131,45 +189,19 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ * @param param6 Parameter 6, as defined by MAV_CMD enum. * @param param7 Parameter 7, as defined by MAV_CMD enum. */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_command_long_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, float param5, float param6, float param7) { - mavlink_header_t hdr; - mavlink_command_long_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_COMMAND_LONG_LEN ) - payload.target_system = target_system; // uint8_t:System which should execute the command - payload.target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - payload.command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - payload.confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - payload.param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - payload.param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - payload.param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - payload.param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - payload.param5 = param5; // float:Parameter 5, as defined by MAV_CMD enum. - payload.param6 = param6; // float:Parameter 6, as defined by MAV_CMD enum. - payload.param7 = param7; // float:Parameter 7, as defined by MAV_CMD enum. - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_COMMAND_LONG_LEN; - hdr.msgid = MAVLINK_MSG_ID_COMMAND_LONG; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x3F, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_command_long_pack_chan_send(chan, msg, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7); } #endif + // MESSAGE COMMAND_LONG UNPACKING + /** * @brief Get field target_system from command_long message * @@ -177,8 +209,7 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 28); } /** @@ -188,8 +219,7 @@ static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 29); } /** @@ -199,8 +229,7 @@ static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlin */ static inline uint8_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (uint8_t)(p->command); + return MAVLINK_MSG_RETURN_uint8_t(msg, 30); } /** @@ -210,8 +239,7 @@ static inline uint8_t mavlink_msg_command_long_get_command(const mavlink_message */ static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (uint8_t)(p->confirmation); + return MAVLINK_MSG_RETURN_uint8_t(msg, 31); } /** @@ -221,8 +249,7 @@ static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_me */ static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param1); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -232,8 +259,7 @@ static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param2); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -243,8 +269,7 @@ static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param3); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -254,8 +279,7 @@ static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param4); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -265,8 +289,7 @@ static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param5); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -276,8 +299,7 @@ static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param6); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -287,8 +309,7 @@ static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param7(const mavlink_message_t* msg) { - mavlink_command_long_t *p = (mavlink_command_long_t *)&msg->payload[0]; - return (float)(p->param7); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -299,5 +320,19 @@ static inline float mavlink_msg_command_long_get_param7(const mavlink_message_t* */ static inline void mavlink_msg_command_long_decode(const mavlink_message_t* msg, mavlink_command_long_t* command_long) { - memcpy( command_long, msg->payload, sizeof(mavlink_command_long_t)); +#if MAVLINK_NEED_BYTE_SWAP + command_long->param1 = mavlink_msg_command_long_get_param1(msg); + command_long->param2 = mavlink_msg_command_long_get_param2(msg); + command_long->param3 = mavlink_msg_command_long_get_param3(msg); + command_long->param4 = mavlink_msg_command_long_get_param4(msg); + command_long->param5 = mavlink_msg_command_long_get_param5(msg); + command_long->param6 = mavlink_msg_command_long_get_param6(msg); + command_long->param7 = mavlink_msg_command_long_get_param7(msg); + command_long->target_system = mavlink_msg_command_long_get_target_system(msg); + command_long->target_component = mavlink_msg_command_long_get_target_component(msg); + command_long->command = mavlink_msg_command_long_get_command(msg); + command_long->confirmation = mavlink_msg_command_long_get_confirmation(msg); +#else + memcpy(command_long, MAVLINK_PAYLOAD(msg), 32); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_short.h b/thirdParty/mavlink/include/common/mavlink_msg_command_short.h index c0538f38bd4dceb3ab5b00a852a1f45592616a58..876e3b82a3c8f362c87b3855b4110a65fbc8491e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_short.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_short.h @@ -1,24 +1,39 @@ // MESSAGE COMMAND_SHORT PACKING #define MAVLINK_MSG_ID_COMMAND_SHORT 75 -#define MAVLINK_MSG_ID_COMMAND_SHORT_LEN 20 -#define MAVLINK_MSG_75_LEN 20 -#define MAVLINK_MSG_ID_COMMAND_SHORT_KEY 0x14 -#define MAVLINK_MSG_75_KEY 0x14 -typedef struct __mavlink_command_short_t +typedef struct __mavlink_command_short_t { - 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) - + 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_short_t; +#define MAVLINK_MSG_ID_COMMAND_SHORT_LEN 20 +#define MAVLINK_MSG_ID_75_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_SHORT { \ + "COMMAND_SHORT", \ + 8, \ + { { "param1", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_short_t, param1) }, \ + { "param2", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_short_t, param2) }, \ + { "param3", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_short_t, param3) }, \ + { "param4", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_short_t, param4) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_command_short_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_command_short_t, target_component) }, \ + { "command", MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_command_short_t, command) }, \ + { "confirmation", MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_command_short_t, confirmation) }, \ + } \ +} + + /** * @brief Pack a command_short message * @param system_id ID of this system @@ -35,25 +50,25 @@ typedef struct __mavlink_command_short_t * @param param4 Parameter 4, as defined by MAV_CMD enum. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_command_short_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) +static inline uint16_t mavlink_msg_command_short_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) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; - 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. + put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. + put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. + put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. + put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 16, target_system); // System which should execute the command + put_uint8_t_by_index(msg, 17, target_component); // Component which should execute the command, 0 for all components + put_uint8_t_by_index(msg, 18, command); // Command ID, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 19, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_SHORT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 160); } /** - * @brief Pack a command_short message + * @brief Pack a command_short message on a channel * @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 @@ -68,22 +83,58 @@ static inline uint16_t mavlink_msg_command_short_pack(uint8_t system_id, uint8_t * @param param4 Parameter 4, as defined by MAV_CMD enum. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_command_short_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) +static inline uint16_t mavlink_msg_command_short_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) +{ + msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; + + put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. + put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. + put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. + put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 16, target_system); // System which should execute the command + put_uint8_t_by_index(msg, 17, target_component); // Component which should execute the command, 0 for all components + put_uint8_t_by_index(msg, 18, command); // Command ID, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 19, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 160); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a command_short message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System which should execute the command + * @param target_component Component which should execute the command, 0 for all components + * @param command Command ID, as defined by MAV_CMD enum. + * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + * @param param1 Parameter 1, as defined by MAV_CMD enum. + * @param param2 Parameter 2, as defined by MAV_CMD enum. + * @param param3 Parameter 3, as defined by MAV_CMD enum. + * @param param4 Parameter 4, as defined by MAV_CMD enum. + */ +static inline void mavlink_msg_command_short_pack_chan_send(mavlink_channel_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) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; - 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. + put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. + put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. + put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. + put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 16, target_system); // System which should execute the command + put_uint8_t_by_index(msg, 17, target_component); // Component which should execute the command, 0 for all components + put_uint8_t_by_index(msg, 18, command); // Command ID, as defined by MAV_CMD enum. + put_uint8_t_by_index(msg, 19, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_SHORT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 20, 160); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a command_short struct into a message @@ -98,8 +149,6 @@ static inline uint16_t mavlink_msg_command_short_encode(uint8_t system_id, uint8 return mavlink_msg_command_short_pack(system_id, component_id, msg, command_short->target_system, command_short->target_component, command_short->command, command_short->confirmation, command_short->param1, command_short->param2, command_short->param3, command_short->param4); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a command_short message * @param chan MAVLink channel to send the message @@ -113,42 +162,19 @@ static inline uint16_t mavlink_msg_command_short_encode(uint8_t system_id, uint8 * @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 + static inline void mavlink_msg_command_short_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_header_t hdr; - mavlink_command_short_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_COMMAND_SHORT_LEN ) - payload.target_system = target_system; // uint8_t:System which should execute the command - payload.target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - payload.command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - payload.confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - payload.param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - payload.param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - payload.param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - payload.param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_COMMAND_SHORT_LEN; - hdr.msgid = MAVLINK_MSG_ID_COMMAND_SHORT; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x14, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_command_short_pack_chan_send(chan, msg, target_system, target_component, command, confirmation, param1, param2, param3, param4); } #endif + // MESSAGE COMMAND_SHORT UNPACKING + /** * @brief Get field target_system from command_short message * @@ -156,8 +182,7 @@ static inline void mavlink_msg_command_short_send(mavlink_channel_t chan, uint8_ */ static inline uint8_t mavlink_msg_command_short_get_target_system(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -167,8 +192,7 @@ static inline uint8_t mavlink_msg_command_short_get_target_system(const mavlink_ */ static inline uint8_t mavlink_msg_command_short_get_target_component(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -178,8 +202,7 @@ static inline uint8_t mavlink_msg_command_short_get_target_component(const mavli */ static inline uint8_t mavlink_msg_command_short_get_command(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (uint8_t)(p->command); + return MAVLINK_MSG_RETURN_uint8_t(msg, 18); } /** @@ -189,8 +212,7 @@ static inline uint8_t mavlink_msg_command_short_get_command(const mavlink_messag */ static inline uint8_t mavlink_msg_command_short_get_confirmation(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (uint8_t)(p->confirmation); + return MAVLINK_MSG_RETURN_uint8_t(msg, 19); } /** @@ -200,8 +222,7 @@ static inline uint8_t mavlink_msg_command_short_get_confirmation(const mavlink_m */ static inline float mavlink_msg_command_short_get_param1(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (float)(p->param1); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -211,8 +232,7 @@ static inline float mavlink_msg_command_short_get_param1(const mavlink_message_t */ static inline float mavlink_msg_command_short_get_param2(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (float)(p->param2); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -222,8 +242,7 @@ static inline float mavlink_msg_command_short_get_param2(const mavlink_message_t */ static inline float mavlink_msg_command_short_get_param3(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (float)(p->param3); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -233,8 +252,7 @@ static inline float mavlink_msg_command_short_get_param3(const mavlink_message_t */ static inline float mavlink_msg_command_short_get_param4(const mavlink_message_t* msg) { - mavlink_command_short_t *p = (mavlink_command_short_t *)&msg->payload[0]; - return (float)(p->param4); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -245,5 +263,16 @@ static inline float mavlink_msg_command_short_get_param4(const mavlink_message_t */ static inline void mavlink_msg_command_short_decode(const mavlink_message_t* msg, mavlink_command_short_t* command_short) { - memcpy( command_short, msg->payload, sizeof(mavlink_command_short_t)); +#if MAVLINK_NEED_BYTE_SWAP + command_short->param1 = mavlink_msg_command_short_get_param1(msg); + command_short->param2 = mavlink_msg_command_short_get_param2(msg); + command_short->param3 = mavlink_msg_command_short_get_param3(msg); + command_short->param4 = mavlink_msg_command_short_get_param4(msg); + command_short->target_system = mavlink_msg_command_short_get_target_system(msg); + command_short->target_component = mavlink_msg_command_short_get_target_component(msg); + command_short->command = mavlink_msg_command_short_get_command(msg); + command_short->confirmation = mavlink_msg_command_short_get_confirmation(msg); +#else + memcpy(command_short, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 681a572598f87500cb55d85b868f013c93900f28..a9ebff3f6b96a64603ecde77a8f675d986ef33ca 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -1,22 +1,17 @@ // 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 -#define MAVLINK_MSG_ID_CONTROL_STATUS_KEY 0xF9 -#define MAVLINK_MSG_52_KEY 0xF9 -typedef struct __mavlink_control_status_t +typedef struct __mavlink_control_status_t { - uint8_t position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - uint8_t vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - uint8_t gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - uint8_t ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent - uint8_t control_att; ///< 0: Attitude control disabled, 1: enabled - uint8_t control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled - uint8_t control_pos_z; ///< 0: Z position control disabled, 1: enabled - uint8_t control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled - + uint8_t position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + uint8_t vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + uint8_t gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + uint8_t ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent + uint8_t control_att; ///< 0: Attitude control disabled, 1: enabled + uint8_t control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled + uint8_t control_pos_z; ///< 0: Z position control disabled, 1: enabled + uint8_t control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled } mavlink_control_status_t; /** @@ -35,25 +30,25 @@ typedef struct __mavlink_control_status_t * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - 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 + put_uint8_t_by_index(position_fix, 0, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + put_uint8_t_by_index(vision_fix, 1, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + put_uint8_t_by_index(gps_fix, 2, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + put_uint8_t_by_index(ahrs_health, 3, msg->payload); // Attitude estimation health: 0: poor, 255: excellent + put_uint8_t_by_index(control_att, 4, msg->payload); // 0: Attitude control disabled, 1: enabled + put_uint8_t_by_index(control_pos_xy, 5, msg->payload); // 0: X, Y position control disabled, 1: enabled + put_uint8_t_by_index(control_pos_z, 6, msg->payload); // 0: Z position control disabled, 1: enabled + put_uint8_t_by_index(control_pos_yaw, 7, msg->payload); // 0: Yaw angle control disabled, 1: enabled - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 8, 102); } /** - * @brief Pack a control_status message + * @brief Pack a control_status message on a channel * @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 @@ -68,22 +63,58 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + + put_uint8_t_by_index(position_fix, 0, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + put_uint8_t_by_index(vision_fix, 1, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + put_uint8_t_by_index(gps_fix, 2, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + put_uint8_t_by_index(ahrs_health, 3, msg->payload); // Attitude estimation health: 0: poor, 255: excellent + put_uint8_t_by_index(control_att, 4, msg->payload); // 0: Attitude control disabled, 1: enabled + put_uint8_t_by_index(control_pos_xy, 5, msg->payload); // 0: X, Y position control disabled, 1: enabled + put_uint8_t_by_index(control_pos_z, 6, msg->payload); // 0: Z position control disabled, 1: enabled + put_uint8_t_by_index(control_pos_yaw, 7, msg->payload); // 0: Yaw angle control disabled, 1: enabled + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 102); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a control_status message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + * @param ahrs_health Attitude estimation health: 0: poor, 255: excellent + * @param control_att 0: Attitude control disabled, 1: enabled + * @param control_pos_xy 0: X, Y position control disabled, 1: enabled + * @param control_pos_z 0: Z position control disabled, 1: enabled + * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled + */ +static inline void mavlink_msg_control_status_pack_chan_send(mavlink_channel_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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - 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 + put_uint8_t_by_index(position_fix, 0, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + put_uint8_t_by_index(vision_fix, 1, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + put_uint8_t_by_index(gps_fix, 2, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + put_uint8_t_by_index(ahrs_health, 3, msg->payload); // Attitude estimation health: 0: poor, 255: excellent + put_uint8_t_by_index(control_att, 4, msg->payload); // 0: Attitude control disabled, 1: enabled + put_uint8_t_by_index(control_pos_xy, 5, msg->payload); // 0: X, Y position control disabled, 1: enabled + put_uint8_t_by_index(control_pos_z, 6, msg->payload); // 0: Z position control disabled, 1: enabled + put_uint8_t_by_index(control_pos_yaw, 7, msg->payload); // 0: Yaw angle control disabled, 1: enabled - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); + mavlink_finalize_message_chan_send(msg, chan, 8, 102); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a control_status struct into a message @@ -98,8 +129,6 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint return mavlink_msg_control_status_pack(system_id, component_id, msg, control_status->position_fix, control_status->vision_fix, control_status->gps_fix, control_status->ahrs_health, control_status->control_att, control_status->control_pos_xy, control_status->control_pos_z, control_status->control_pos_yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a control_status message * @param chan MAVLink channel to send the message @@ -113,42 +142,19 @@ 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 + 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_header_t hdr; - mavlink_control_status_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN ) - payload.position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - payload.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 - payload.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 - payload.ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent - payload.control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled - payload.control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled - payload.control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_control_status_pack_chan_send(chan, msg, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw); } #endif + // MESSAGE CONTROL_STATUS UNPACKING + /** * @brief Get field position_fix from control_status message * @@ -156,8 +162,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->position_fix); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -167,8 +172,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->vision_fix); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -178,8 +182,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->gps_fix); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -189,8 +192,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->ahrs_health); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -200,8 +202,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->control_att); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -211,8 +212,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->control_pos_xy); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -222,8 +222,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->control_pos_z); + return MAVLINK_MSG_RETURN_uint8_t(msg, 6); } /** @@ -233,8 +232,7 @@ 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) { - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; - return (uint8_t)(p->control_pos_yaw); + return MAVLINK_MSG_RETURN_uint8_t(msg, 7); } /** @@ -245,5 +243,16 @@ 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) { - memcpy( control_status, msg->payload, sizeof(mavlink_control_status_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(control_status, msg->payload, 8); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h index bd1b7c02f026adf82546964ee1422569c6e32944..1ff8b9a889ef39092df7c6a3049a7e374627abc0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h @@ -1,19 +1,29 @@ // MESSAGE DATA_STREAM PACKING #define MAVLINK_MSG_ID_DATA_STREAM 67 -#define MAVLINK_MSG_ID_DATA_STREAM_LEN 4 -#define MAVLINK_MSG_67_LEN 4 -#define MAVLINK_MSG_ID_DATA_STREAM_KEY 0x50 -#define MAVLINK_MSG_67_KEY 0x50 -typedef struct __mavlink_data_stream_t +typedef struct __mavlink_data_stream_t { - uint16_t message_rate; ///< The requested interval between two messages of this type - uint8_t stream_id; ///< The ID of the requested data stream - uint8_t on_off; ///< 1 stream is enabled, 0 stream is stopped. - + uint16_t message_rate; ///< The requested interval between two messages of this type + uint8_t stream_id; ///< The ID of the requested data stream + uint8_t on_off; ///< 1 stream is enabled, 0 stream is stopped. } mavlink_data_stream_t; +#define MAVLINK_MSG_ID_DATA_STREAM_LEN 4 +#define MAVLINK_MSG_ID_67_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_DATA_STREAM { \ + "DATA_STREAM", \ + 3, \ + { { "message_rate", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_data_stream_t, message_rate) }, \ + { "stream_id", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_data_stream_t, stream_id) }, \ + { "on_off", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_data_stream_t, on_off) }, \ + } \ +} + + /** * @brief Pack a data_stream message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_data_stream_t * @param on_off 1 stream is enabled, 0 stream is stopped. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t stream_id, uint16_t message_rate, uint8_t on_off) { - mavlink_data_stream_t *p = (mavlink_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; - p->stream_id = stream_id; // uint8_t:The ID of the requested data stream - p->message_rate = message_rate; // uint16_t:The requested interval between two messages of this type - p->on_off = on_off; // uint8_t:1 stream is enabled, 0 stream is stopped. + put_uint16_t_by_index(msg, 0, message_rate); // The requested interval between two messages of this type + put_uint8_t_by_index(msg, 2, stream_id); // The ID of the requested data stream + put_uint8_t_by_index(msg, 3, on_off); // 1 stream is enabled, 0 stream is stopped. - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 21); } /** - * @brief Pack a data_stream message + * @brief Pack a data_stream message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t c * @param on_off 1 stream is enabled, 0 stream is stopped. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint16_t message_rate,uint8_t on_off) { - mavlink_data_stream_t *p = (mavlink_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; - p->stream_id = stream_id; // uint8_t:The ID of the requested data stream - p->message_rate = message_rate; // uint16_t:The requested interval between two messages of this type - p->on_off = on_off; // uint8_t:1 stream is enabled, 0 stream is stopped. + put_uint16_t_by_index(msg, 0, message_rate); // The requested interval between two messages of this type + put_uint8_t_by_index(msg, 2, stream_id); // The ID of the requested data stream + put_uint8_t_by_index(msg, 3, on_off); // 1 stream is enabled, 0 stream is stopped. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_STREAM_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 21); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a data_stream message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param stream_id The ID of the requested data stream + * @param message_rate The requested interval between two messages of this type + * @param on_off 1 stream is enabled, 0 stream is stopped. + */ +static inline void mavlink_msg_data_stream_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint16_t message_rate,uint8_t on_off) +{ + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; + + put_uint16_t_by_index(msg, 0, message_rate); // The requested interval between two messages of this type + put_uint8_t_by_index(msg, 2, stream_id); // The ID of the requested data stream + put_uint8_t_by_index(msg, 3, on_off); // 1 stream is enabled, 0 stream is stopped. + + mavlink_finalize_message_chan_send(msg, chan, 4, 21); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a data_stream struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t return mavlink_msg_data_stream_pack(system_id, component_id, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a data_stream message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t * @param message_rate The requested interval between two messages of this type * @param on_off 1 stream is enabled, 0 stream is stopped. */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) { - mavlink_header_t hdr; - mavlink_data_stream_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DATA_STREAM_LEN ) - payload.stream_id = stream_id; // uint8_t:The ID of the requested data stream - payload.message_rate = message_rate; // uint16_t:The requested interval between two messages of this type - payload.on_off = on_off; // uint8_t:1 stream is enabled, 0 stream is stopped. - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_DATA_STREAM_LEN; - hdr.msgid = MAVLINK_MSG_ID_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x50, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_data_stream_pack_chan_send(chan, msg, stream_id, message_rate, on_off); } #endif + // MESSAGE DATA_STREAM UNPACKING + /** * @brief Get field stream_id from data_stream message * @@ -121,8 +137,7 @@ static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_message_t* msg) { - mavlink_data_stream_t *p = (mavlink_data_stream_t *)&msg->payload[0]; - return (uint8_t)(p->stream_id); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -132,8 +147,7 @@ static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_messag */ static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_message_t* msg) { - mavlink_data_stream_t *p = (mavlink_data_stream_t *)&msg->payload[0]; - return (uint16_t)(p->message_rate); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -143,8 +157,7 @@ static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_me */ static inline uint8_t mavlink_msg_data_stream_get_on_off(const mavlink_message_t* msg) { - mavlink_data_stream_t *p = (mavlink_data_stream_t *)&msg->payload[0]; - return (uint8_t)(p->on_off); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -155,5 +168,11 @@ static inline uint8_t mavlink_msg_data_stream_get_on_off(const mavlink_message_t */ static inline void mavlink_msg_data_stream_decode(const mavlink_message_t* msg, mavlink_data_stream_t* data_stream) { - memcpy( data_stream, msg->payload, sizeof(mavlink_data_stream_t)); +#if MAVLINK_NEED_BYTE_SWAP + data_stream->message_rate = mavlink_msg_data_stream_get_message_rate(msg); + data_stream->stream_id = mavlink_msg_data_stream_get_stream_id(msg); + data_stream->on_off = mavlink_msg_data_stream_get_on_off(msg); +#else + memcpy(data_stream, MAVLINK_PAYLOAD(msg), 4); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index e125d8e9fb40ca8a016856528c820fdc119d2485..5aafb0987b8d1ed5706e1abb16eb518430e70e34 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -1,18 +1,27 @@ // MESSAGE DEBUG PACKING #define MAVLINK_MSG_ID_DEBUG 255 -#define MAVLINK_MSG_ID_DEBUG_LEN 5 -#define MAVLINK_MSG_255_LEN 5 -#define MAVLINK_MSG_ID_DEBUG_KEY 0x54 -#define MAVLINK_MSG_255_KEY 0x54 -typedef struct __mavlink_debug_t +typedef struct __mavlink_debug_t { - float value; ///< DEBUG value - uint8_t ind; ///< index of debug variable - + float value; ///< DEBUG value + uint8_t ind; ///< index of debug variable } mavlink_debug_t; +#define MAVLINK_MSG_ID_DEBUG_LEN 5 +#define MAVLINK_MSG_ID_255_LEN 5 + + + +#define MAVLINK_MESSAGE_INFO_DEBUG { \ + "DEBUG", \ + 2, \ + { { "value", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_debug_t, value) }, \ + { "ind", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_debug_t, ind) }, \ + } \ +} + + /** * @brief Pack a debug message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_debug_t * @param value DEBUG value * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value + put_float_by_index(msg, 0, value); // DEBUG value + put_uint8_t_by_index(msg, 4, ind); // index of debug variable - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 5, 127); } /** - * @brief Pack a debug message + * @brief Pack a debug message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone * @param value DEBUG value * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_DEBUG; + + put_float_by_index(msg, 0, value); // DEBUG value + put_uint8_t_by_index(msg, 4, ind); // index of debug variable + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 127); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a debug message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param ind index of debug variable + * @param value DEBUG value + */ +static inline void mavlink_msg_debug_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t ind,float value) { - mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value + put_float_by_index(msg, 0, value); // DEBUG value + put_uint8_t_by_index(msg, 4, ind); // index of debug variable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); + mavlink_finalize_message_chan_send(msg, chan, 5, 127); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a debug struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo return mavlink_msg_debug_pack(system_id, component_id, msg, debug->ind, debug->value); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a debug message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { - mavlink_header_t hdr; - mavlink_debug_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DEBUG_LEN ) - payload.ind = ind; // uint8_t:index of debug variable - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x54, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 5); + mavlink_msg_debug_pack_chan_send(chan, msg, ind, value); } #endif + // MESSAGE DEBUG UNPACKING + /** * @brief Get field ind from debug message * @@ -114,8 +128,7 @@ 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) { - mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; - return (uint8_t)(p->ind); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; - return (float)(p->value); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( debug, msg->payload, sizeof(mavlink_debug_t)); +#if MAVLINK_NEED_BYTE_SWAP + debug->value = mavlink_msg_debug_get_value(msg); + debug->ind = mavlink_msg_debug_get_ind(msg); +#else + memcpy(debug, MAVLINK_PAYLOAD(msg), 5); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 75114ec8fb4e72cb110eb1ffda66b1df7eeb7966..4fe0d8eb26e9ff8c10e3194ae5e48316d1a1b891 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -1,22 +1,33 @@ // 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 -#define MAVLINK_MSG_ID_DEBUG_VECT_KEY 0x2B -#define MAVLINK_MSG_251_KEY 0x2B -typedef struct __mavlink_debug_vect_t +typedef struct __mavlink_debug_vect_t { - uint64_t usec; ///< Timestamp - float x; ///< x - float y; ///< y - float z; ///< z - 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_ID_DEBUG_VECT_LEN 30 +#define MAVLINK_MSG_ID_251_LEN 30 + #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 +#define MAVLINK_MESSAGE_INFO_DEBUG_VECT { \ + "DEBUG_VECT", \ + 5, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_vect_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_debug_vect_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_debug_vect_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_debug_vect_t, z) }, \ + { "name", MAVLINK_TYPE_CHAR, 10, 20, offsetof(mavlink_debug_vect_t, name) }, \ + } \ +} + + /** * @brief Pack a debug_vect message * @param system_id ID of this system @@ -30,22 +41,22 @@ typedef struct __mavlink_debug_vect_t * @param z z * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp + put_float_by_index(msg, 8, x); // x + put_float_by_index(msg, 12, y); // y + put_float_by_index(msg, 16, z); // z + put_char_array_by_index(msg, 20, name, 10); // Name - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 30, 15); } /** - * @brief Pack a debug_vect message + * @brief Pack a debug_vect message on a channel * @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 @@ -57,20 +68,50 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co * @param z z * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp + put_float_by_index(msg, 8, x); // x + put_float_by_index(msg, 12, y); // y + put_float_by_index(msg, 16, z); // z + put_char_array_by_index(msg, 20, name, 10); // Name - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 15); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a debug_vect message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param name Name + * @param usec Timestamp + * @param x x + * @param y y + * @param z z + */ +static inline void mavlink_msg_debug_vect_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + const char *name,uint64_t usec,float x,float y,float z) +{ + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp + put_float_by_index(msg, 8, x); // x + put_float_by_index(msg, 12, y); // y + put_float_by_index(msg, 16, z); // z + put_char_array_by_index(msg, 20, name, 10); // Name + + mavlink_finalize_message_chan_send(msg, chan, 30, 15); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a debug_vect struct into a message * @@ -84,8 +125,6 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->usec, debug_vect->x, debug_vect->y, debug_vect->z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a debug_vect message * @param chan MAVLink channel to send the message @@ -96,50 +135,27 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param y y * @param z z */ -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) +#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_header_t hdr; - mavlink_debug_vect_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN ) - memcpy(payload.name, name, sizeof(payload.name)); // char[10]:Name - payload.usec = usec; // uint64_t:Timestamp - payload.x = x; // float:x - payload.y = y; // float:y - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x2B, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 30); + mavlink_msg_debug_vect_pack_chan_send(chan, msg, name, usec, x, y, z); } #endif + // MESSAGE DEBUG_VECT UNPACKING + /** * @brief Get field name from debug_vect message * * @return Name */ -static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char* name) +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(name, p->name, sizeof(p->name)); - return sizeof(p->name); + return MAVLINK_MSG_RETURN_char_array(msg, name, 10, 20); } /** @@ -149,8 +165,7 @@ 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) { - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +175,7 @@ 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) { - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +185,7 @@ 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) { - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +195,7 @@ 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) { - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -194,5 +206,13 @@ 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) { - memcpy( debug_vect, msg->payload, sizeof(mavlink_debug_vect_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + mavlink_msg_debug_vect_get_name(msg, debug_vect->name); +#else + memcpy(debug_vect, MAVLINK_PAYLOAD(msg), 30); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index d4d13dc202d35c671e3fb38ff051277c74a39b6d..6f99c355bb3fa09066e6d7c7eabacbe21dbe1a0d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_KEY 0x15 -#define MAVLINK_MSG_33_KEY 0x15 -typedef struct __mavlink_global_position_t +typedef struct __mavlink_global_position_t { - uint64_t usec; ///< Timestamp (microseconds since unix epoch) - float lat; ///< Latitude, in degrees - float lon; ///< Longitude, in degrees - float alt; ///< Absolute altitude, in meters - float vx; ///< X Speed (in Latitude direction, positive: going north) - float vy; ///< Y Speed (in Longitude direction, positive: going east) - float vz; ///< Z Speed (in Altitude direction, positive: going up) - + uint64_t usec; ///< Timestamp (microseconds since unix epoch) + float lat; ///< Latitude, in degrees + float lon; ///< Longitude, in degrees + float alt; ///< Absolute altitude, in meters + float vx; ///< X Speed (in Latitude direction, positive: going north) + float vy; ///< Y Speed (in Longitude direction, positive: going east) + float vz; ///< Z Speed (in Altitude direction, positive: going up) } mavlink_global_position_t; +#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 +#define MAVLINK_MSG_ID_33_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION { \ + "GLOBAL_POSITION", \ + 7, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_t, usec) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_position_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_position_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_t, alt) }, \ + { "vx", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_t, vx) }, \ + { "vy", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_t, vy) }, \ + { "vz", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_t, vz) }, \ + } \ +} + + /** * @brief Pack a global_position message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_global_position_t * @param vz Z Speed (in Altitude direction, positive: going up) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since unix epoch) + put_float_by_index(msg, 8, lat); // Latitude, in degrees + put_float_by_index(msg, 12, lon); // Longitude, in degrees + put_float_by_index(msg, 16, alt); // Absolute altitude, in meters + put_float_by_index(msg, 20, vx); // X Speed (in Latitude direction, positive: going north) + put_float_by_index(msg, 24, vy); // Y Speed (in Longitude direction, positive: going east) + put_float_by_index(msg, 28, vz); // Z Speed (in Altitude direction, positive: going up) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 147); } /** - * @brief Pack a global_position message + * @brief Pack a global_position message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 * @param vz Z Speed (in Altitude direction, positive: going up) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since unix epoch) + put_float_by_index(msg, 8, lat); // Latitude, in degrees + put_float_by_index(msg, 12, lon); // Longitude, in degrees + put_float_by_index(msg, 16, alt); // Absolute altitude, in meters + put_float_by_index(msg, 20, vx); // X Speed (in Latitude direction, positive: going north) + put_float_by_index(msg, 24, vy); // Y Speed (in Longitude direction, positive: going east) + put_float_by_index(msg, 28, vz); // Z Speed (in Altitude direction, positive: going up) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 147); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a global_position message on a channel and send + * @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) + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) + */ +static inline void mavlink_msg_global_position_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float lat,float lon,float alt,float vx,float vy,float vz) +{ + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since unix epoch) + put_float_by_index(msg, 8, lat); // Latitude, in degrees + put_float_by_index(msg, 12, lon); // Longitude, in degrees + put_float_by_index(msg, 16, alt); // Absolute altitude, in meters + put_float_by_index(msg, 20, vx); // X Speed (in Latitude direction, positive: going north) + put_float_by_index(msg, 24, vy); // Y Speed (in Longitude direction, positive: going east) + put_float_by_index(msg, 28, vz); // Z Speed (in Altitude direction, positive: going up) + + mavlink_finalize_message_chan_send(msg, chan, 32, 147); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a global_position struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin return mavlink_msg_global_position_pack(system_id, component_id, msg, global_position->usec, global_position->lat, global_position->lon, global_position->alt, global_position->vx, global_position->vy, global_position->vz); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a global_position message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_global_position_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) - payload.lat = lat; // float:Latitude, in degrees - payload.lon = lon; // float:Longitude, in degrees - payload.alt = alt; // float:Absolute altitude, in meters - payload.vx = vx; // float:X Speed (in Latitude direction, positive: going north) - payload.vy = vy; // float:Y Speed (in Longitude direction, positive: going east) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x15, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_global_position_pack_chan_send(chan, msg, usec, lat, lon, alt, vx, vy, vz); } #endif + // MESSAGE GLOBAL_POSITION UNPACKING + /** * @brief Get field usec from global_position message * @@ -149,8 +173,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (float)(p->lat); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (float)(p->lon); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (float)(p->alt); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (float)(p->vx); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (float)(p->vy); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; - return (float)(p->vz); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( global_position, msg->payload, sizeof(mavlink_global_position_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(global_position, MAVLINK_PAYLOAD(msg), 32); +#endif } 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 4943298eec56ccfa643df15a4c7f07581fa66dbc..6edfa952aa3935ce3b503e5704487a7f0219e333 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_KEY 0xD4 -#define MAVLINK_MSG_73_KEY 0xD4 -typedef struct __mavlink_global_position_int_t +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), 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 - + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + 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; +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 20 +#define MAVLINK_MSG_ID_73_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \ + "GLOBAL_POSITION_INT", \ + 7, \ + { { "lat", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_int_t, lat) }, \ + { "lon", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lon) }, \ + { "alt", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, alt) }, \ + { "vx", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_int_t, vx) }, \ + { "vy", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_global_position_int_t, vy) }, \ + { "vz", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_global_position_int_t, vz) }, \ + { "hdg", MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_global_position_int_t, hdg) }, \ + } \ +} + + /** * @brief Pack a global_position_int message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_global_position_int_t * @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, uint16_t hdg) +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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - 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 + put_int32_t_by_index(msg, 0, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 4, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 8, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL + put_int16_t_by_index(msg, 12, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 14, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 16, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_uint16_t_by_index(msg, 18, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 241); } /** - * @brief Pack a global_position_int message + * @brief Pack a global_position_int message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @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, uint16_t hdg) +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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - 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 + put_int32_t_by_index(msg, 0, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 4, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 8, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL + put_int16_t_by_index(msg, 12, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 14, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 16, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_uint16_t_by_index(msg, 18, hdg); // 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, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 241); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a global_position_int message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @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), 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 + */ +static inline void mavlink_msg_global_position_int_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + + put_int32_t_by_index(msg, 0, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 4, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 8, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL + put_int16_t_by_index(msg, 12, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 14, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 16, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_uint16_t_by_index(msg, 18, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + + mavlink_finalize_message_chan_send(msg, chan, 20, 241); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a global_position_int struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, 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); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a global_position_int message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @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, uint16_t hdg) { - mavlink_header_t hdr; - mavlink_global_position_int_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN ) - payload.lat = lat; // int32_t:Latitude, expressed as * 1E7 - payload.lon = lon; // int32_t:Longitude, expressed as * 1E7 - payload.alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL - payload.vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - payload.vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - payload.vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xD4, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_global_position_int_pack_chan_send(chan, msg, lat, lon, alt, vx, vy, vz, hdg); } #endif + // MESSAGE GLOBAL_POSITION_INT UNPACKING + /** * @brief Get field lat from global_position_int message * @@ -149,8 +173,7 @@ 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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; - return (int32_t)(p->lat); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; - return (int32_t)(p->lon); + return MAVLINK_MSG_RETURN_int32_t(msg, 4); } /** @@ -171,8 +193,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; - return (int32_t)(p->alt); + return MAVLINK_MSG_RETURN_int32_t(msg, 8); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; - return (int16_t)(p->vx); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; - return (int16_t)(p->vy); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; - return (int16_t)(p->vz); + return MAVLINK_MSG_RETURN_int16_t(msg, 16); } /** @@ -215,8 +233,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 18); } /** @@ -227,5 +244,15 @@ static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_mes */ static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) { - memcpy( global_position_int, msg->payload, sizeof(mavlink_global_position_int_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + global_position_int->hdg = mavlink_msg_global_position_int_get_hdg(msg); +#else + memcpy(global_position_int, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h index cfd814cd2aedf71e2699c5be51680d4d7e8e3a66..8788ad5200c354d015b0279be28f34b09ae9eb89 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h @@ -1,20 +1,31 @@ // MESSAGE GLOBAL_POSITION_SETPOINT_INT PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT 52 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN 14 -#define MAVLINK_MSG_52_LEN 14 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_KEY 0x14 -#define MAVLINK_MSG_52_KEY 0x14 -typedef struct __mavlink_global_position_setpoint_int_t +typedef struct __mavlink_global_position_setpoint_int_t { - int32_t latitude; ///< WGS84 Latitude position in degrees * 1E7 - int32_t longitude; ///< WGS84 Longitude position in degrees * 1E7 - int32_t altitude; ///< WGS84 Altitude in meters * 1000 (positive for up) - int16_t yaw; ///< Desired yaw angle in degrees * 100 - + int32_t latitude; ///< WGS84 Latitude position in degrees * 1E7 + int32_t longitude; ///< WGS84 Longitude position in degrees * 1E7 + int32_t altitude; ///< WGS84 Altitude in meters * 1000 (positive for up) + int16_t yaw; ///< Desired yaw angle in degrees * 100 } mavlink_global_position_setpoint_int_t; +#define MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN 14 +#define MAVLINK_MSG_ID_52_LEN 14 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT { \ + "GLOBAL_POSITION_SETPOINT_INT", \ + 4, \ + { { "latitude", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_setpoint_int_t, latitude) }, \ + { "longitude", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_setpoint_int_t, longitude) }, \ + { "altitude", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_setpoint_int_t, altitude) }, \ + { "yaw", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_setpoint_int_t, yaw) }, \ + } \ +} + + /** * @brief Pack a global_position_setpoint_int message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_global_position_setpoint_int_t * @param yaw Desired yaw angle in degrees * 100 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) +static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) { - mavlink_global_position_setpoint_int_t *p = (mavlink_global_position_setpoint_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; - p->latitude = latitude; // int32_t:WGS84 Latitude position in degrees * 1E7 - p->longitude = longitude; // int32_t:WGS84 Longitude position in degrees * 1E7 - p->altitude = altitude; // int32_t:WGS84 Altitude in meters * 1000 (positive for up) - p->yaw = yaw; // int16_t:Desired yaw angle in degrees * 100 + put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 + put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 + put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) + put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 14, 142); } /** - * @brief Pack a global_position_setpoint_int message + * @brief Pack a global_position_setpoint_int message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t sys * @param yaw Desired yaw angle in degrees * 100 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_global_position_setpoint_int_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, int16_t yaw) +static inline uint16_t mavlink_msg_global_position_setpoint_int_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,int16_t yaw) +{ + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + + put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 + put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 + put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) + put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 142); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a global_position_setpoint_int message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param latitude WGS84 Latitude position in degrees * 1E7 + * @param longitude WGS84 Longitude position in degrees * 1E7 + * @param altitude WGS84 Altitude in meters * 1000 (positive for up) + * @param yaw Desired yaw angle in degrees * 100 + */ +static inline void mavlink_msg_global_position_setpoint_int_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) { - mavlink_global_position_setpoint_int_t *p = (mavlink_global_position_setpoint_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; - p->latitude = latitude; // int32_t:WGS84 Latitude position in degrees * 1E7 - p->longitude = longitude; // int32_t:WGS84 Longitude position in degrees * 1E7 - p->altitude = altitude; // int32_t:WGS84 Altitude in meters * 1000 (positive for up) - p->yaw = yaw; // int16_t:Desired yaw angle in degrees * 100 + put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 + put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 + put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) + put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 14, 142); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a global_position_setpoint_int struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s return mavlink_msg_global_position_setpoint_int_pack(system_id, component_id, msg, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a global_position_setpoint_int message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s * @param altitude WGS84 Altitude in meters * 1000 (positive for up) * @param yaw Desired yaw angle in degrees * 100 */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) { - mavlink_header_t hdr; - mavlink_global_position_setpoint_int_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN ) - payload.latitude = latitude; // int32_t:WGS84 Latitude position in degrees * 1E7 - payload.longitude = longitude; // int32_t:WGS84 Longitude position in degrees * 1E7 - payload.altitude = altitude; // int32_t:WGS84 Altitude in meters * 1000 (positive for up) - payload.yaw = yaw; // int16_t:Desired yaw angle in degrees * 100 - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN; - hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x14, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 14); + mavlink_msg_global_position_setpoint_int_pack_chan_send(chan, msg, latitude, longitude, altitude, yaw); } #endif + // MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING + /** * @brief Get field latitude from global_position_setpoint_int message * @@ -128,8 +146,7 @@ static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel */ static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) { - mavlink_global_position_setpoint_int_t *p = (mavlink_global_position_setpoint_int_t *)&msg->payload[0]; - return (int32_t)(p->latitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -139,8 +156,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(cons */ static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) { - mavlink_global_position_setpoint_int_t *p = (mavlink_global_position_setpoint_int_t *)&msg->payload[0]; - return (int32_t)(p->longitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 4); } /** @@ -150,8 +166,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(con */ static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) { - mavlink_global_position_setpoint_int_t *p = (mavlink_global_position_setpoint_int_t *)&msg->payload[0]; - return (int32_t)(p->altitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 8); } /** @@ -161,8 +176,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(cons */ static inline int16_t mavlink_msg_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) { - mavlink_global_position_setpoint_int_t *p = (mavlink_global_position_setpoint_int_t *)&msg->payload[0]; - return (int16_t)(p->yaw); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -173,5 +187,12 @@ static inline int16_t mavlink_msg_global_position_setpoint_int_get_yaw(const mav */ static inline void mavlink_msg_global_position_setpoint_int_decode(const mavlink_message_t* msg, mavlink_global_position_setpoint_int_t* global_position_setpoint_int) { - memcpy( global_position_setpoint_int, msg->payload, sizeof(mavlink_global_position_setpoint_int_t)); +#if MAVLINK_NEED_BYTE_SWAP + global_position_setpoint_int->latitude = mavlink_msg_global_position_setpoint_int_get_latitude(msg); + global_position_setpoint_int->longitude = mavlink_msg_global_position_setpoint_int_get_longitude(msg); + global_position_setpoint_int->altitude = mavlink_msg_global_position_setpoint_int_get_altitude(msg); + global_position_setpoint_int->yaw = mavlink_msg_global_position_setpoint_int_get_yaw(msg); +#else + memcpy(global_position_setpoint_int, MAVLINK_PAYLOAD(msg), 14); +#endif } 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 1daa52ea6d7998c24264b30b800e6872fac1c1be..4365fa82bdf36b76b7b1de6503680dc3f9c96aef 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,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_KEY 0x3C -#define MAVLINK_MSG_49_KEY 0x3C -typedef struct __mavlink_gps_local_origin_set_t +typedef struct __mavlink_gps_local_origin_set_t { - int32_t latitude; ///< Latitude (WGS84), expressed as * 1E7 - int32_t longitude; ///< Longitude (WGS84), expressed as * 1E7 - int32_t altitude; ///< Altitude(WGS84), expressed as * 1000 - + int32_t latitude; ///< Latitude (WGS84), expressed as * 1E7 + int32_t longitude; ///< Longitude (WGS84), expressed as * 1E7 + int32_t altitude; ///< Altitude(WGS84), expressed as * 1000 } mavlink_gps_local_origin_set_t; +#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN 12 +#define MAVLINK_MSG_ID_49_LEN 12 + + + +#define MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET { \ + "GPS_LOCAL_ORIGIN_SET", \ + 3, \ + { { "latitude", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_local_origin_set_t, latitude) }, \ + { "longitude", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_local_origin_set_t, longitude) }, \ + { "altitude", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_local_origin_set_t, altitude) }, \ + } \ +} + + /** * @brief Pack a gps_local_origin_set message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_gps_local_origin_set_t * @param altitude Altitude(WGS84), expressed as * 1000 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_int32_t_by_index(msg, 0, latitude); // Latitude (WGS84), expressed as * 1E7 + put_int32_t_by_index(msg, 4, longitude); // Longitude (WGS84), expressed as * 1E7 + put_int32_t_by_index(msg, 8, altitude); // Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 12, 14); } /** - * @brief Pack a gps_local_origin_set message + * @brief Pack a gps_local_origin_set message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, * @param altitude Altitude(WGS84), expressed as * 1000 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_int32_t_by_index(msg, 0, latitude); // Latitude (WGS84), expressed as * 1E7 + put_int32_t_by_index(msg, 4, longitude); // Longitude (WGS84), expressed as * 1E7 + put_int32_t_by_index(msg, 8, altitude); // Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 14); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a gps_local_origin_set message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param latitude Latitude (WGS84), expressed as * 1E7 + * @param longitude Longitude (WGS84), expressed as * 1E7 + * @param altitude Altitude(WGS84), expressed as * 1000 + */ +static inline void mavlink_msg_gps_local_origin_set_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + int32_t latitude,int32_t longitude,int32_t altitude) +{ + msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + + put_int32_t_by_index(msg, 0, latitude); // Latitude (WGS84), expressed as * 1E7 + put_int32_t_by_index(msg, 4, longitude); // Longitude (WGS84), expressed as * 1E7 + put_int32_t_by_index(msg, 8, altitude); // Altitude(WGS84), expressed as * 1000 + + mavlink_finalize_message_chan_send(msg, chan, 12, 14); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a gps_local_origin_set struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id return mavlink_msg_gps_local_origin_set_pack(system_id, component_id, msg, gps_local_origin_set->latitude, gps_local_origin_set->longitude, gps_local_origin_set->altitude); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_local_origin_set message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { - mavlink_header_t hdr; - mavlink_gps_local_origin_set_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN ) - payload.latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 - payload.longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x3C, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 12); + mavlink_msg_gps_local_origin_set_pack_chan_send(chan, msg, latitude, longitude, altitude); } #endif + // MESSAGE GPS_LOCAL_ORIGIN_SET UNPACKING + /** * @brief Get field latitude from gps_local_origin_set message * @@ -121,8 +137,7 @@ 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) { - mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; - return (int32_t)(p->latitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; - return (int32_t)(p->longitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 4); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; - return (int32_t)(p->altitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 8); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( gps_local_origin_set, msg->payload, sizeof(mavlink_gps_local_origin_set_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(gps_local_origin_set, MAVLINK_PAYLOAD(msg), 12); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 3dabc300510821b7523c5952874a13cd550409b9..223056e8c08599baf7630332a07a611dab5ea4ef 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -1,26 +1,43 @@ // 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 -#define MAVLINK_MSG_ID_GPS_RAW_KEY 0x5B -#define MAVLINK_MSG_32_KEY 0x5B -typedef struct __mavlink_gps_raw_t +typedef struct __mavlink_gps_raw_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float lat; ///< Latitude in degrees - float lon; ///< Longitude in degrees - float alt; ///< Altitude in meters - float eph; ///< GPS HDOP - 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 - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float lat; ///< Latitude in degrees + float lon; ///< Longitude in degrees + float alt; ///< Altitude in meters + float eph; ///< GPS HDOP + 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; +#define MAVLINK_MSG_ID_GPS_RAW_LEN 38 +#define MAVLINK_MSG_ID_32_LEN 38 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RAW { \ + "GPS_RAW", \ + 10, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_t, usec) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gps_raw_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gps_raw_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gps_raw_t, alt) }, \ + { "eph", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gps_raw_t, eph) }, \ + { "epv", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gps_raw_t, epv) }, \ + { "v", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gps_raw_t, v) }, \ + { "hdg", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gps_raw_t, hdg) }, \ + { "fix_type", MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_gps_raw_t, fix_type) }, \ + { "satellites_visible", MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_gps_raw_t, satellites_visible) }, \ + } \ +} + + /** * @brief Pack a gps_raw message * @param system_id ID of this system @@ -39,27 +56,27 @@ typedef struct __mavlink_gps_raw_t * @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, uint8_t satellites_visible) +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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - 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, MAVLINK_MSG_ID_GPS_RAW_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, lat); // Latitude in degrees + put_float_by_index(msg, 12, lon); // Longitude in degrees + put_float_by_index(msg, 16, alt); // Altitude in meters + put_float_by_index(msg, 20, eph); // GPS HDOP + put_float_by_index(msg, 24, epv); // GPS VDOP + put_float_by_index(msg, 28, v); // GPS ground speed + put_float_by_index(msg, 32, hdg); // Compass heading in degrees, 0..360 degrees + put_uint8_t_by_index(msg, 36, 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. + put_uint8_t_by_index(msg, 37, satellites_visible); // Number of satellites visible + + return mavlink_finalize_message(msg, system_id, component_id, 38, 198); } /** - * @brief Pack a gps_raw message + * @brief Pack a gps_raw message on a channel * @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 @@ -76,24 +93,64 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo * @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, uint8_t satellites_visible) +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) +{ + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, lat); // Latitude in degrees + put_float_by_index(msg, 12, lon); // Longitude in degrees + put_float_by_index(msg, 16, alt); // Altitude in meters + put_float_by_index(msg, 20, eph); // GPS HDOP + put_float_by_index(msg, 24, epv); // GPS VDOP + put_float_by_index(msg, 28, v); // GPS ground speed + put_float_by_index(msg, 32, hdg); // Compass heading in degrees, 0..360 degrees + put_uint8_t_by_index(msg, 36, 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. + put_uint8_t_by_index(msg, 37, satellites_visible); // Number of satellites visible + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 38, 198); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a gps_raw message on a channel and send + * @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 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 degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @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 + */ +static inline void mavlink_msg_gps_raw_pack_chan_send(mavlink_channel_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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - 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, MAVLINK_MSG_ID_GPS_RAW_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, lat); // Latitude in degrees + put_float_by_index(msg, 12, lon); // Longitude in degrees + put_float_by_index(msg, 16, alt); // Altitude in meters + put_float_by_index(msg, 20, eph); // GPS HDOP + put_float_by_index(msg, 24, epv); // GPS VDOP + put_float_by_index(msg, 28, v); // GPS ground speed + put_float_by_index(msg, 32, hdg); // Compass heading in degrees, 0..360 degrees + put_uint8_t_by_index(msg, 36, 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. + put_uint8_t_by_index(msg, 37, satellites_visible); // Number of satellites visible + + mavlink_finalize_message_chan_send(msg, chan, 38, 198); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a gps_raw struct into a message @@ -108,8 +165,6 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com 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); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_raw message * @param chan MAVLink channel to send the message @@ -125,44 +180,19 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @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, uint8_t satellites_visible) { - mavlink_header_t hdr; - mavlink_gps_raw_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_RAW_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.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. - payload.lat = lat; // float:Latitude in degrees - payload.lon = lon; // float:Longitude in degrees - payload.alt = alt; // float:Altitude in meters - payload.eph = eph; // float:GPS HDOP - payload.epv = epv; // float:GPS VDOP - payload.v = v; // float:GPS ground speed - payload.hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x5B, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 38); + mavlink_msg_gps_raw_pack_chan_send(chan, msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg, satellites_visible); } #endif + // MESSAGE GPS_RAW UNPACKING + /** * @brief Get field usec from gps_raw message * @@ -170,8 +200,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -181,8 +210,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (uint8_t)(p->fix_type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 36); } /** @@ -192,8 +220,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->lat); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -203,8 +230,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->lon); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -214,8 +240,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->alt); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -225,8 +250,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->eph); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -236,8 +260,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->epv); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -247,8 +270,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->v); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -258,8 +280,7 @@ 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) { - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; - return (float)(p->hdg); + return MAVLINK_MSG_RETURN_float(msg, 32); } /** @@ -269,8 +290,7 @@ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) */ 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); + return MAVLINK_MSG_RETURN_uint8_t(msg, 37); } /** @@ -281,5 +301,18 @@ static inline uint8_t mavlink_msg_gps_raw_get_satellites_visible(const mavlink_m */ static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavlink_gps_raw_t* gps_raw) { - memcpy( gps_raw, msg->payload, sizeof(mavlink_gps_raw_t)); +#if MAVLINK_NEED_BYTE_SWAP + gps_raw->usec = mavlink_msg_gps_raw_get_usec(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); + gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); + gps_raw->satellites_visible = mavlink_msg_gps_raw_get_satellites_visible(msg); +#else + memcpy(gps_raw, MAVLINK_PAYLOAD(msg), 38); +#endif } 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 fcdd41ad1d10fb508a424e9aa8dedbb2d1b0f3fe..7cac3537bfef03fe29d749815566d31b34d77dcf 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -1,26 +1,43 @@ // 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 -#define MAVLINK_MSG_ID_GPS_RAW_INT_KEY 0xA6 -#define MAVLINK_MSG_25_KEY 0xA6 -typedef struct __mavlink_gps_raw_int_t +typedef struct __mavlink_gps_raw_int_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int32_t lat; ///< Latitude in 1E7 degrees - int32_t lon; ///< Longitude in 1E7 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 - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude in 1E7 degrees + int32_t lon; ///< Longitude in 1E7 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; +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 +#define MAVLINK_MSG_ID_25_LEN 30 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ + "GPS_RAW_INT", \ + 10, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, usec) }, \ + { "lat", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ + { "lon", MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_raw_int_t, lon) }, \ + { "alt", MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_raw_int_t, alt) }, \ + { "eph", MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_gps_raw_int_t, eph) }, \ + { "epv", MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_gps_raw_int_t, epv) }, \ + { "vel", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ + { "hdg", MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, hdg) }, \ + { "fix_type", MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ + { "satellites_visible", MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ + } \ +} + + /** * @brief Pack a gps_raw_int message * @param system_id ID of this system @@ -39,27 +56,27 @@ typedef struct __mavlink_gps_raw_int_t * @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, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) +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) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - 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, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int32_t_by_index(msg, 8, lat); // Latitude in 1E7 degrees + put_int32_t_by_index(msg, 12, lon); // Longitude in 1E7 degrees + put_int32_t_by_index(msg, 16, alt); // Altitude in 1E3 meters (millimeters) above MSL + put_uint16_t_by_index(msg, 20, eph); // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 22, epv); // GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 24, vel); // GPS ground speed (m/s * 100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 26, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + put_uint8_t_by_index(msg, 28, 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. + put_uint8_t_by_index(msg, 29, satellites_visible); // Number of satellites visible. If unknown, set to 255 + + return mavlink_finalize_message(msg, system_id, component_id, 30, 185); } /** - * @brief Pack a gps_raw_int message + * @brief Pack a gps_raw_int message on a channel * @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 @@ -76,24 +93,64 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @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, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) +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) +{ + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int32_t_by_index(msg, 8, lat); // Latitude in 1E7 degrees + put_int32_t_by_index(msg, 12, lon); // Longitude in 1E7 degrees + put_int32_t_by_index(msg, 16, alt); // Altitude in 1E3 meters (millimeters) above MSL + put_uint16_t_by_index(msg, 20, eph); // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 22, epv); // GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 24, vel); // GPS ground speed (m/s * 100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 26, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + put_uint8_t_by_index(msg, 28, 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. + put_uint8_t_by_index(msg, 29, satellites_visible); // Number of satellites visible. If unknown, set to 255 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 185); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a gps_raw_int message on a channel and send + * @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 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) 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 + */ +static inline void mavlink_msg_gps_raw_int_pack_chan_send(mavlink_channel_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) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - 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, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int32_t_by_index(msg, 8, lat); // Latitude in 1E7 degrees + put_int32_t_by_index(msg, 12, lon); // Longitude in 1E7 degrees + put_int32_t_by_index(msg, 16, alt); // Altitude in 1E3 meters (millimeters) above MSL + put_uint16_t_by_index(msg, 20, eph); // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 22, epv); // GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 24, vel); // GPS ground speed (m/s * 100). If unknown, set to: 65535 + put_uint16_t_by_index(msg, 26, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + put_uint8_t_by_index(msg, 28, 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. + put_uint8_t_by_index(msg, 29, satellites_visible); // Number of satellites visible. If unknown, set to 255 + + mavlink_finalize_message_chan_send(msg, chan, 30, 185); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a gps_raw_int struct into a message @@ -108,8 +165,6 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t 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); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_raw_int message * @param chan MAVLink channel to send the message @@ -125,44 +180,19 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @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, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { - mavlink_header_t hdr; - mavlink_gps_raw_int_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.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. - payload.lat = lat; // int32_t:Latitude in 1E7 degrees - payload.lon = lon; // int32_t:Longitude in 1E7 degrees - payload.alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL - payload.eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - payload.epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - payload.vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 - payload.hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA6, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 30); + mavlink_msg_gps_raw_int_pack_chan_send(chan, msg, usec, fix_type, lat, lon, alt, eph, epv, vel, hdg, satellites_visible); } #endif + // MESSAGE GPS_RAW_INT UNPACKING + /** * @brief Get field usec from gps_raw_int message * @@ -170,8 +200,7 @@ 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) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -181,8 +210,7 @@ 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) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (uint8_t)(p->fix_type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 28); } /** @@ -192,8 +220,7 @@ 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) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (int32_t)(p->lat); + return MAVLINK_MSG_RETURN_int32_t(msg, 8); } /** @@ -203,8 +230,7 @@ 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) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (int32_t)(p->lon); + return MAVLINK_MSG_RETURN_int32_t(msg, 12); } /** @@ -214,8 +240,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (int32_t)(p->alt); + return MAVLINK_MSG_RETURN_int32_t(msg, 16); } /** @@ -225,8 +250,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m */ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (uint16_t)(p->eph); + return MAVLINK_MSG_RETURN_uint16_t(msg, 20); } /** @@ -236,8 +260,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (uint16_t)(p->epv); + return MAVLINK_MSG_RETURN_uint16_t(msg, 22); } /** @@ -247,8 +270,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* */ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (uint16_t)(p->vel); + return MAVLINK_MSG_RETURN_uint16_t(msg, 24); } /** @@ -258,8 +280,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 26); } /** @@ -269,8 +290,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg) { - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (uint8_t)(p->satellites_visible); + return MAVLINK_MSG_RETURN_uint8_t(msg, 29); } /** @@ -281,5 +301,18 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavli */ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int) { - memcpy( gps_raw_int, msg->payload, sizeof(mavlink_gps_raw_int_t)); +#if MAVLINK_NEED_BYTE_SWAP + gps_raw_int->usec = mavlink_msg_gps_raw_int_get_usec(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->vel = mavlink_msg_gps_raw_int_get_vel(msg); + gps_raw_int->hdg = mavlink_msg_gps_raw_int_get_hdg(msg); + gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); + gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg); +#else + memcpy(gps_raw_int, MAVLINK_PAYLOAD(msg), 30); +#endif } 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 b4d471170a1319a42bd62ada4a632f41ba9a848a..cd6b0a3f029851c494af2903c7b67bcf85b4ddb2 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,21 +1,33 @@ // 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 -#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_KEY 0x8E -#define MAVLINK_MSG_48_KEY 0x8E -typedef struct __mavlink_gps_set_global_origin_t +typedef struct __mavlink_gps_set_global_origin_t { - 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 - + 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; +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 +#define MAVLINK_MSG_ID_48_LEN 14 + + + +#define MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN { \ + "GPS_SET_GLOBAL_ORIGIN", \ + 5, \ + { { "latitude", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_set_global_origin_t, latitude) }, \ + { "longitude", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_set_global_origin_t, longitude) }, \ + { "altitude", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_set_global_origin_t, altitude) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gps_set_global_origin_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_gps_set_global_origin_t, target_component) }, \ + } \ +} + + /** * @brief Pack a gps_set_global_origin message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_gps_set_global_origin_t * @param altitude global position * 1000 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_int32_t_by_index(msg, 0, latitude); // global position * 1E7 + put_int32_t_by_index(msg, 4, longitude); // global position * 1E7 + put_int32_t_by_index(msg, 8, altitude); // global position * 1000 + put_uint8_t_by_index(msg, 12, target_system); // System ID + put_uint8_t_by_index(msg, 13, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 14, 170); } /** - * @brief Pack a gps_set_global_origin message + * @brief Pack a gps_set_global_origin message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, * @param altitude global position * 1000 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_int32_t_by_index(msg, 0, latitude); // global position * 1E7 + put_int32_t_by_index(msg, 4, longitude); // global position * 1E7 + put_int32_t_by_index(msg, 8, altitude); // global position * 1000 + put_uint8_t_by_index(msg, 12, target_system); // System ID + put_uint8_t_by_index(msg, 13, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 170); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a gps_set_global_origin message on a channel and send + * @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 latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 + */ +static inline void mavlink_msg_gps_set_global_origin_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int32_t latitude,int32_t longitude,int32_t altitude) +{ + msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + + put_int32_t_by_index(msg, 0, latitude); // global position * 1E7 + put_int32_t_by_index(msg, 4, longitude); // global position * 1E7 + put_int32_t_by_index(msg, 8, altitude); // global position * 1000 + put_uint8_t_by_index(msg, 12, target_system); // System ID + put_uint8_t_by_index(msg, 13, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 14, 170); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a gps_set_global_origin struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->latitude, gps_set_global_origin->longitude, gps_set_global_origin->altitude); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_set_global_origin message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ 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 + 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_header_t hdr; - mavlink_gps_set_global_origin_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.latitude = latitude; // int32_t:global position * 1E7 - payload.longitude = longitude; // int32_t:global position * 1E7 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x8E, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 14); + mavlink_msg_gps_set_global_origin_pack_chan_send(chan, msg, target_system, target_component, latitude, longitude, altitude); } #endif + // MESSAGE GPS_SET_GLOBAL_ORIGIN UNPACKING + /** * @brief Get field target_system from gps_set_global_origin message * @@ -135,8 +155,7 @@ 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) { - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 12); } /** @@ -146,8 +165,7 @@ 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) { - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 13); } /** @@ -157,8 +175,7 @@ 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) { - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; - return (int32_t)(p->latitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -168,8 +185,7 @@ 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) { - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; - return (int32_t)(p->longitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 4); } /** @@ -179,8 +195,7 @@ 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) { - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; - return (int32_t)(p->altitude); + return MAVLINK_MSG_RETURN_int32_t(msg, 8); } /** @@ -191,5 +206,13 @@ 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) { - memcpy( gps_set_global_origin, msg->payload, sizeof(mavlink_gps_set_global_origin_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + 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); +#else + memcpy(gps_set_global_origin, MAVLINK_PAYLOAD(msg), 14); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index 1c020abe29107d4fed64553adde7378abfc81491..ef8f96c8bf06d0d9cba66d85a76d992d88010dbe 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -1,27 +1,39 @@ // 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 -#define MAVLINK_MSG_ID_GPS_STATUS_KEY 0x63 -#define MAVLINK_MSG_27_KEY 0x63 -typedef struct __mavlink_gps_status_t +typedef struct __mavlink_gps_status_t { - uint8_t satellites_visible; ///< Number of satellites visible - 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 - + uint8_t satellites_visible; ///< Number of satellites visible + 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_ID_GPS_STATUS_LEN 101 +#define MAVLINK_MSG_ID_27_LEN 101 + #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 +#define MAVLINK_MESSAGE_INFO_GPS_STATUS { \ + "GPS_STATUS", \ + 6, \ + { { "satellites_visible", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_status_t, satellites_visible) }, \ + { "satellite_prn", MAVLINK_TYPE_UINT8_T, 20, 1, offsetof(mavlink_gps_status_t, satellite_prn) }, \ + { "satellite_used", MAVLINK_TYPE_UINT8_T, 20, 21, offsetof(mavlink_gps_status_t, satellite_used) }, \ + { "satellite_elevation", MAVLINK_TYPE_UINT8_T, 20, 41, offsetof(mavlink_gps_status_t, satellite_elevation) }, \ + { "satellite_azimuth", MAVLINK_TYPE_UINT8_T, 20, 61, offsetof(mavlink_gps_status_t, satellite_azimuth) }, \ + { "satellite_snr", MAVLINK_TYPE_UINT8_T, 20, 81, offsetof(mavlink_gps_status_t, satellite_snr) }, \ + } \ +} + + /** * @brief Pack a gps_status message * @param system_id ID of this system @@ -36,23 +48,23 @@ 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 uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_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) { - mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - 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 + put_uint8_t_by_index(msg, 0, satellites_visible); // Number of satellites visible + put_uint8_t_array_by_index(msg, 1, satellite_prn, 20); // Global satellite ID + put_uint8_t_array_by_index(msg, 21, satellite_used, 20); // 0: Satellite not used, 1: used for localization + put_uint8_t_array_by_index(msg, 41, satellite_elevation, 20); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite + put_uint8_t_array_by_index(msg, 61, satellite_azimuth, 20); // Direction of satellite, 0: 0 deg, 255: 360 deg. + put_uint8_t_array_by_index(msg, 81, satellite_snr, 20); // Signal to noise ratio of satellite - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 101, 23); } /** - * @brief Pack a gps_status message + * @brief Pack a gps_status message on a channel * @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 @@ -65,20 +77,52 @@ 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 uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_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) +{ + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; + + put_uint8_t_by_index(msg, 0, satellites_visible); // Number of satellites visible + put_uint8_t_array_by_index(msg, 1, satellite_prn, 20); // Global satellite ID + put_uint8_t_array_by_index(msg, 21, satellite_used, 20); // 0: Satellite not used, 1: used for localization + put_uint8_t_array_by_index(msg, 41, satellite_elevation, 20); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite + put_uint8_t_array_by_index(msg, 61, satellite_azimuth, 20); // Direction of satellite, 0: 0 deg, 255: 360 deg. + put_uint8_t_array_by_index(msg, 81, satellite_snr, 20); // Signal to noise ratio of satellite + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 101, 23); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a gps_status message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + */ +static inline void mavlink_msg_gps_status_pack_chan_send(mavlink_channel_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) { - mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - 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 + put_uint8_t_by_index(msg, 0, satellites_visible); // Number of satellites visible + put_uint8_t_array_by_index(msg, 1, satellite_prn, 20); // Global satellite ID + put_uint8_t_array_by_index(msg, 21, satellite_used, 20); // 0: Satellite not used, 1: used for localization + put_uint8_t_array_by_index(msg, 41, satellite_elevation, 20); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite + put_uint8_t_array_by_index(msg, 61, satellite_azimuth, 20); // Direction of satellite, 0: 0 deg, 255: 360 deg. + put_uint8_t_array_by_index(msg, 81, satellite_snr, 20); // Signal to noise ratio of satellite - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); + mavlink_finalize_message_chan_send(msg, chan, 101, 23); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a gps_status struct into a message @@ -93,8 +137,6 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_status message * @param chan MAVLink channel to send the message @@ -106,40 +148,19 @@ 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 */ -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) +#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_header_t hdr; - mavlink_gps_status_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_STATUS_LEN ) - payload.satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - memcpy(payload.satellite_prn, satellite_prn, sizeof(payload.satellite_prn)); // uint8_t[20]:Global satellite ID - memcpy(payload.satellite_used, satellite_used, sizeof(payload.satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization - memcpy(payload.satellite_elevation, satellite_elevation, sizeof(payload.satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite - memcpy(payload.satellite_azimuth, satellite_azimuth, sizeof(payload.satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. - memcpy(payload.satellite_snr, satellite_snr, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x63, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 101); + mavlink_msg_gps_status_pack_chan_send(chan, msg, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr); } #endif + // MESSAGE GPS_STATUS UNPACKING + /** * @brief Get field satellites_visible from gps_status message * @@ -147,8 +168,7 @@ 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) { - mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - return (uint8_t)(p->satellites_visible); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -156,12 +176,9 @@ 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, uint8_t* satellite_prn) +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(satellite_prn, p->satellite_prn, sizeof(p->satellite_prn)); - return sizeof(p->satellite_prn); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_prn, 20, 1); } /** @@ -169,12 +186,9 @@ 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, uint8_t* satellite_used) +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(satellite_used, p->satellite_used, sizeof(p->satellite_used)); - return sizeof(p->satellite_used); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_used, 20, 21); } /** @@ -182,12 +196,9 @@ 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, uint8_t* satellite_elevation) +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(satellite_elevation, p->satellite_elevation, sizeof(p->satellite_elevation)); - return sizeof(p->satellite_elevation); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_elevation, 20, 41); } /** @@ -195,12 +206,9 @@ 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, uint8_t* satellite_azimuth) +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(satellite_azimuth, p->satellite_azimuth, sizeof(p->satellite_azimuth)); - return sizeof(p->satellite_azimuth); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_azimuth, 20, 61); } /** @@ -208,12 +216,9 @@ 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, uint8_t* satellite_snr) +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(satellite_snr, p->satellite_snr, sizeof(p->satellite_snr)); - return sizeof(p->satellite_snr); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_snr, 20, 81); } /** @@ -224,5 +229,14 @@ 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) { - memcpy( gps_status, msg->payload, sizeof(mavlink_gps_status_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(gps_status, MAVLINK_PAYLOAD(msg), 101); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 942f033cf74624b13909b1cde2d2200707cc1cc0..19bc0a34ae3fc7eab09911d561490e50b652ecd1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -1,24 +1,39 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 -#define MAVLINK_MSG_ID_HEARTBEAT_LEN 8 -#define MAVLINK_MSG_0_LEN 8 -#define MAVLINK_MSG_ID_HEARTBEAT_KEY 0xA -#define MAVLINK_MSG_0_KEY 0xA -typedef struct __mavlink_heartbeat_t +typedef struct __mavlink_heartbeat_t { - uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - uint8_t autopilot; ///< Autopilot type / class. defined in MAV_CLASS ENUM - uint8_t system_mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - uint8_t flight_mode; ///< Navigation mode, see MAV_FLIGHT_MODE ENUM - uint8_t system_status; ///< System status flag, see MAV_STATUS ENUM - uint8_t safety_status; ///< System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - uint8_t link_status; ///< Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - uint8_t mavlink_version; ///< MAVLink version - + uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + uint8_t autopilot; ///< Autopilot type / class. defined in MAV_CLASS ENUM + uint8_t system_mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t flight_mode; ///< Navigation mode, see MAV_FLIGHT_MODE ENUM + uint8_t system_status; ///< System status flag, see MAV_STATUS ENUM + uint8_t safety_status; ///< System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + uint8_t link_status; ///< Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + uint8_t mavlink_version; ///< MAVLink version } mavlink_heartbeat_t; +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 8 +#define MAVLINK_MSG_ID_0_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ + "HEARTBEAT", \ + 8, \ + { { "type", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_heartbeat_t, type) }, \ + { "autopilot", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_heartbeat_t, autopilot) }, \ + { "system_mode", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_heartbeat_t, system_mode) }, \ + { "flight_mode", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_heartbeat_t, flight_mode) }, \ + { "system_status", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, system_status) }, \ + { "safety_status", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, safety_status) }, \ + { "link_status", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, link_status) }, \ + { "mavlink_version", MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ + } \ +} + + /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -34,25 +49,25 @@ typedef struct __mavlink_heartbeat_t * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint8_t system_mode, uint8_t flight_mode, uint8_t system_status, uint8_t safety_status, uint8_t link_status) +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, uint8_t system_mode, uint8_t flight_mode, uint8_t system_status, uint8_t safety_status, uint8_t link_status) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - 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:Autopilot type / class. defined in MAV_CLASS ENUM - p->system_mode = system_mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->flight_mode = flight_mode; // uint8_t:Navigation mode, see MAV_FLIGHT_MODE ENUM - p->system_status = system_status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->safety_status = safety_status; // uint8_t:System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - p->link_status = link_status; // uint8_t:Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM + put_uint8_t_by_index(msg, 2, system_mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, flight_mode); // Navigation mode, see MAV_FLIGHT_MODE ENUM + put_uint8_t_by_index(msg, 4, system_status); // System status flag, see MAV_STATUS ENUM + put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 7, 3); // MAVLink version - 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); + return mavlink_finalize_message(msg, system_id, component_id, 8, 153); } /** - * @brief Pack a heartbeat message + * @brief Pack a heartbeat message on a channel * @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 @@ -66,23 +81,58 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint8_t system_mode, uint8_t flight_mode, uint8_t system_status, uint8_t safety_status, uint8_t link_status) +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,uint8_t system_mode,uint8_t flight_mode,uint8_t system_status,uint8_t safety_status,uint8_t link_status) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - 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:Autopilot type / class. defined in MAV_CLASS ENUM - p->system_mode = system_mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->flight_mode = flight_mode; // uint8_t:Navigation mode, see MAV_FLIGHT_MODE ENUM - p->system_status = system_status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->safety_status = safety_status; // uint8_t:System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - p->link_status = link_status; // uint8_t:Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM + put_uint8_t_by_index(msg, 2, system_mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, flight_mode); // Navigation mode, see MAV_FLIGHT_MODE ENUM + put_uint8_t_by_index(msg, 4, system_status); // System status flag, see MAV_STATUS ENUM + put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 7, 3); // MAVLink version - 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); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 153); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a heartbeat message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_CLASS ENUM + * @param system_mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + * @param flight_mode Navigation mode, see MAV_FLIGHT_MODE ENUM + * @param system_status System status flag, see MAV_STATUS ENUM + * @param safety_status System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + */ +static inline void mavlink_msg_heartbeat_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t autopilot,uint8_t system_mode,uint8_t flight_mode,uint8_t system_status,uint8_t safety_status,uint8_t link_status) +{ + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + + put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM + put_uint8_t_by_index(msg, 2, system_mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, flight_mode); // Navigation mode, see MAV_FLIGHT_MODE ENUM + put_uint8_t_by_index(msg, 4, system_status); // System status flag, see MAV_STATUS ENUM + put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 7, 3); // MAVLink version + + mavlink_finalize_message_chan_send(msg, chan, 8, 153); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a heartbeat struct into a message * @@ -96,8 +146,6 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->system_mode, heartbeat->flight_mode, heartbeat->system_status, heartbeat->safety_status, heartbeat->link_status); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -110,42 +158,19 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param safety_status System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t system_mode, uint8_t flight_mode, uint8_t system_status, uint8_t safety_status, uint8_t link_status) { - mavlink_header_t hdr; - mavlink_heartbeat_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HEARTBEAT_LEN ) - payload.type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - payload.autopilot = autopilot; // uint8_t:Autopilot type / class. defined in MAV_CLASS ENUM - payload.system_mode = system_mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - payload.flight_mode = flight_mode; // uint8_t:Navigation mode, see MAV_FLIGHT_MODE ENUM - payload.system_status = system_status; // uint8_t:System status flag, see MAV_STATUS ENUM - payload.safety_status = safety_status; // uint8_t:System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - payload.link_status = link_status; // uint8_t:Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_heartbeat_pack_chan_send(chan, msg, type, autopilot, system_mode, flight_mode, system_status, safety_status, link_status); } #endif + // MESSAGE HEARTBEAT UNPACKING + /** * @brief Get field type from heartbeat message * @@ -153,8 +178,7 @@ 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) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -164,8 +188,7 @@ 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) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->autopilot); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -175,8 +198,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_system_mode(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->system_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -186,8 +208,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_system_mode(const mavlink_messag */ static inline uint8_t mavlink_msg_heartbeat_get_flight_mode(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->flight_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -197,8 +218,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_flight_mode(const mavlink_messag */ static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->system_status); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -208,8 +228,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_mess */ static inline uint8_t mavlink_msg_heartbeat_get_safety_status(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->safety_status); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -219,8 +238,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_safety_status(const mavlink_mess */ static inline uint8_t mavlink_msg_heartbeat_get_link_status(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->link_status); + return MAVLINK_MSG_RETURN_uint8_t(msg, 6); } /** @@ -230,8 +248,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_link_status(const mavlink_messag */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->mavlink_version); + return MAVLINK_MSG_RETURN_uint8_t(msg, 7); } /** @@ -242,5 +259,16 @@ 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) { - memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); +#if MAVLINK_NEED_BYTE_SWAP + heartbeat->type = mavlink_msg_heartbeat_get_type(msg); + heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); + heartbeat->system_mode = mavlink_msg_heartbeat_get_system_mode(msg); + heartbeat->flight_mode = mavlink_msg_heartbeat_get_flight_mode(msg); + heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); + heartbeat->safety_status = mavlink_msg_heartbeat_get_safety_status(msg); + heartbeat->link_status = mavlink_msg_heartbeat_get_link_status(msg); + heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); +#else + memcpy(heartbeat, MAVLINK_PAYLOAD(msg), 8); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h index d06b40bfd6a21c77b53bc35bf69fa604ed89c80d..3d5b53f68382ac928119d9cb4bc1b9d75dc29327 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h @@ -1,27 +1,45 @@ // MESSAGE HIL_CONTROLS PACKING #define MAVLINK_MSG_ID_HIL_CONTROLS 91 -#define MAVLINK_MSG_ID_HIL_CONTROLS_LEN 42 -#define MAVLINK_MSG_91_LEN 42 -#define MAVLINK_MSG_ID_HIL_CONTROLS_KEY 0x67 -#define MAVLINK_MSG_91_KEY 0x67 -typedef struct __mavlink_hil_controls_t +typedef struct __mavlink_hil_controls_t { - uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll_ailerons; ///< Control output -1 .. 1 - float pitch_elevator; ///< Control output -1 .. 1 - float yaw_rudder; ///< Control output -1 .. 1 - float throttle; ///< Throttle 0 .. 1 - float aux1; ///< Aux 1, -1 .. 1 - float aux2; ///< Aux 2, -1 .. 1 - float aux3; ///< Aux 3, -1 .. 1 - float aux4; ///< Aux 4, -1 .. 1 - uint8_t mode; ///< System mode (MAV_MODE) - uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE) - + uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll_ailerons; ///< Control output -1 .. 1 + float pitch_elevator; ///< Control output -1 .. 1 + float yaw_rudder; ///< Control output -1 .. 1 + float throttle; ///< Throttle 0 .. 1 + float aux1; ///< Aux 1, -1 .. 1 + float aux2; ///< Aux 2, -1 .. 1 + float aux3; ///< Aux 3, -1 .. 1 + float aux4; ///< Aux 4, -1 .. 1 + uint8_t mode; ///< System mode (MAV_MODE) + uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE) } mavlink_hil_controls_t; +#define MAVLINK_MSG_ID_HIL_CONTROLS_LEN 42 +#define MAVLINK_MSG_ID_91_LEN 42 + + + +#define MAVLINK_MESSAGE_INFO_HIL_CONTROLS { \ + "HIL_CONTROLS", \ + 11, \ + { { "time_us", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_controls_t, time_us) }, \ + { "roll_ailerons", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_controls_t, roll_ailerons) }, \ + { "pitch_elevator", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_controls_t, pitch_elevator) }, \ + { "yaw_rudder", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_controls_t, yaw_rudder) }, \ + { "throttle", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_controls_t, throttle) }, \ + { "aux1", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_controls_t, aux1) }, \ + { "aux2", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_controls_t, aux2) }, \ + { "aux3", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_controls_t, aux3) }, \ + { "aux4", MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_controls_t, aux4) }, \ + { "mode", MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_hil_controls_t, mode) }, \ + { "nav_mode", MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_hil_controls_t, nav_mode) }, \ + } \ +} + + /** * @brief Pack a hil_controls message * @param system_id ID of this system @@ -41,28 +59,28 @@ typedef struct __mavlink_hil_controls_t * @param nav_mode Navigation mode (MAV_NAV_MODE) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - p->time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll_ailerons = roll_ailerons; // float:Control output -1 .. 1 - p->pitch_elevator = pitch_elevator; // float:Control output -1 .. 1 - p->yaw_rudder = yaw_rudder; // float:Control output -1 .. 1 - p->throttle = throttle; // float:Throttle 0 .. 1 - p->aux1 = aux1; // float:Aux 1, -1 .. 1 - p->aux2 = aux2; // float:Aux 2, -1 .. 1 - p->aux3 = aux3; // float:Aux 3, -1 .. 1 - p->aux4 = aux4; // float:Aux 4, -1 .. 1 - p->mode = mode; // uint8_t:System mode (MAV_MODE) - p->nav_mode = nav_mode; // uint8_t:Navigation mode (MAV_NAV_MODE) - - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll_ailerons); // Control output -1 .. 1 + put_float_by_index(msg, 12, pitch_elevator); // Control output -1 .. 1 + put_float_by_index(msg, 16, yaw_rudder); // Control output -1 .. 1 + put_float_by_index(msg, 20, throttle); // Throttle 0 .. 1 + put_float_by_index(msg, 24, aux1); // Aux 1, -1 .. 1 + put_float_by_index(msg, 28, aux2); // Aux 2, -1 .. 1 + put_float_by_index(msg, 32, aux3); // Aux 3, -1 .. 1 + put_float_by_index(msg, 36, aux4); // Aux 4, -1 .. 1 + put_uint8_t_by_index(msg, 40, mode); // System mode (MAV_MODE) + put_uint8_t_by_index(msg, 41, nav_mode); // Navigation mode (MAV_NAV_MODE) + + return mavlink_finalize_message(msg, system_id, component_id, 42, 250); } /** - * @brief Pack a hil_controls message + * @brief Pack a hil_controls message on a channel * @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 @@ -80,26 +98,68 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t * @param nav_mode Navigation mode (MAV_NAV_MODE) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_us,float roll_ailerons,float pitch_elevator,float yaw_rudder,float throttle,float aux1,float aux2,float aux3,float aux4,uint8_t mode,uint8_t nav_mode) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - p->time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll_ailerons = roll_ailerons; // float:Control output -1 .. 1 - p->pitch_elevator = pitch_elevator; // float:Control output -1 .. 1 - p->yaw_rudder = yaw_rudder; // float:Control output -1 .. 1 - p->throttle = throttle; // float:Throttle 0 .. 1 - p->aux1 = aux1; // float:Aux 1, -1 .. 1 - p->aux2 = aux2; // float:Aux 2, -1 .. 1 - p->aux3 = aux3; // float:Aux 3, -1 .. 1 - p->aux4 = aux4; // float:Aux 4, -1 .. 1 - p->mode = mode; // uint8_t:System mode (MAV_MODE) - p->nav_mode = nav_mode; // uint8_t:Navigation mode (MAV_NAV_MODE) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll_ailerons); // Control output -1 .. 1 + put_float_by_index(msg, 12, pitch_elevator); // Control output -1 .. 1 + put_float_by_index(msg, 16, yaw_rudder); // Control output -1 .. 1 + put_float_by_index(msg, 20, throttle); // Throttle 0 .. 1 + put_float_by_index(msg, 24, aux1); // Aux 1, -1 .. 1 + put_float_by_index(msg, 28, aux2); // Aux 2, -1 .. 1 + put_float_by_index(msg, 32, aux3); // Aux 3, -1 .. 1 + put_float_by_index(msg, 36, aux4); // Aux 4, -1 .. 1 + put_uint8_t_by_index(msg, 40, mode); // System mode (MAV_MODE) + put_uint8_t_by_index(msg, 41, nav_mode); // Navigation mode (MAV_NAV_MODE) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 250); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a hil_controls message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -1 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param aux1 Aux 1, -1 .. 1 + * @param aux2 Aux 2, -1 .. 1 + * @param aux3 Aux 3, -1 .. 1 + * @param aux4 Aux 4, -1 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + */ +static inline void mavlink_msg_hil_controls_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t time_us,float roll_ailerons,float pitch_elevator,float yaw_rudder,float throttle,float aux1,float aux2,float aux3,float aux4,uint8_t mode,uint8_t nav_mode) +{ + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; + + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll_ailerons); // Control output -1 .. 1 + put_float_by_index(msg, 12, pitch_elevator); // Control output -1 .. 1 + put_float_by_index(msg, 16, yaw_rudder); // Control output -1 .. 1 + put_float_by_index(msg, 20, throttle); // Throttle 0 .. 1 + put_float_by_index(msg, 24, aux1); // Aux 1, -1 .. 1 + put_float_by_index(msg, 28, aux2); // Aux 2, -1 .. 1 + put_float_by_index(msg, 32, aux3); // Aux 3, -1 .. 1 + put_float_by_index(msg, 36, aux4); // Aux 4, -1 .. 1 + put_uint8_t_by_index(msg, 40, mode); // System mode (MAV_MODE) + put_uint8_t_by_index(msg, 41, nav_mode); // Navigation mode (MAV_NAV_MODE) + + mavlink_finalize_message_chan_send(msg, chan, 42, 250); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a hil_controls struct into a message * @@ -113,8 +173,6 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_us, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a hil_controls message * @param chan MAVLink channel to send the message @@ -131,45 +189,19 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ * @param mode System mode (MAV_MODE) * @param nav_mode Navigation mode (MAV_NAV_MODE) */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) { - mavlink_header_t hdr; - mavlink_hil_controls_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HIL_CONTROLS_LEN ) - payload.time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.roll_ailerons = roll_ailerons; // float:Control output -1 .. 1 - payload.pitch_elevator = pitch_elevator; // float:Control output -1 .. 1 - payload.yaw_rudder = yaw_rudder; // float:Control output -1 .. 1 - payload.throttle = throttle; // float:Throttle 0 .. 1 - payload.aux1 = aux1; // float:Aux 1, -1 .. 1 - payload.aux2 = aux2; // float:Aux 2, -1 .. 1 - payload.aux3 = aux3; // float:Aux 3, -1 .. 1 - payload.aux4 = aux4; // float:Aux 4, -1 .. 1 - payload.mode = mode; // uint8_t:System mode (MAV_MODE) - payload.nav_mode = nav_mode; // uint8_t:Navigation mode (MAV_NAV_MODE) - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_HIL_CONTROLS_LEN; - hdr.msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x67, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 42); + mavlink_msg_hil_controls_pack_chan_send(chan, msg, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode); } #endif + // MESSAGE HIL_CONTROLS UNPACKING + /** * @brief Get field time_us from hil_controls message * @@ -177,8 +209,7 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (uint64_t)(p->time_us); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -188,8 +219,7 @@ static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_messag */ static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->roll_ailerons); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -199,8 +229,7 @@ static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_mes */ static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->pitch_elevator); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -210,8 +239,7 @@ static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_me */ static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->yaw_rudder); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -221,8 +249,7 @@ static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_messag */ static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->throttle); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -232,8 +259,7 @@ static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_ */ static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->aux1); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -243,8 +269,7 @@ static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* m */ static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->aux2); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -254,8 +279,7 @@ static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* m */ static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->aux3); + return MAVLINK_MSG_RETURN_float(msg, 32); } /** @@ -265,8 +289,7 @@ static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* m */ static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (float)(p->aux4); + return MAVLINK_MSG_RETURN_float(msg, 36); } /** @@ -276,8 +299,7 @@ static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (uint8_t)(p->mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 40); } /** @@ -287,8 +309,7 @@ static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* */ static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) { - mavlink_hil_controls_t *p = (mavlink_hil_controls_t *)&msg->payload[0]; - return (uint8_t)(p->nav_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 41); } /** @@ -299,5 +320,19 @@ static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_messag */ static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls) { - memcpy( hil_controls, msg->payload, sizeof(mavlink_hil_controls_t)); +#if MAVLINK_NEED_BYTE_SWAP + hil_controls->time_us = mavlink_msg_hil_controls_get_time_us(msg); + hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg); + hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg); + hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg); + hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg); + hil_controls->aux1 = mavlink_msg_hil_controls_get_aux1(msg); + hil_controls->aux2 = mavlink_msg_hil_controls_get_aux2(msg); + hil_controls->aux3 = mavlink_msg_hil_controls_get_aux3(msg); + hil_controls->aux4 = mavlink_msg_hil_controls_get_aux4(msg); + hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg); + hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg); +#else + memcpy(hil_controls, MAVLINK_PAYLOAD(msg), 42); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h index 1db288a77aa151a9d56f35da663a55b98b1c0903..434c01b8d7a9ec5a273cf974dc389836af825368 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h @@ -1,30 +1,51 @@ // MESSAGE HIL_RC_INPUTS_RAW PACKING #define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW 92 -#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN 33 -#define MAVLINK_MSG_92_LEN 33 -#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_KEY 0x3B -#define MAVLINK_MSG_92_KEY 0x3B -typedef struct __mavlink_hil_rc_inputs_raw_t +typedef struct __mavlink_hil_rc_inputs_raw_t { - uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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 - uint16_t chan9_raw; ///< RC channel 9 value, in microseconds - uint16_t chan10_raw; ///< RC channel 10 value, in microseconds - uint16_t chan11_raw; ///< RC channel 11 value, in microseconds - uint16_t chan12_raw; ///< RC channel 12 value, in microseconds - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - + uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 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 + uint16_t chan9_raw; ///< RC channel 9 value, in microseconds + uint16_t chan10_raw; ///< RC channel 10 value, in microseconds + uint16_t chan11_raw; ///< RC channel 11 value, in microseconds + uint16_t chan12_raw; ///< RC channel 12 value, in microseconds + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% } mavlink_hil_rc_inputs_raw_t; +#define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN 33 +#define MAVLINK_MSG_ID_92_LEN 33 + + + +#define MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW { \ + "HIL_RC_INPUTS_RAW", \ + 14, \ + { { "time_us", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_rc_inputs_raw_t, time_us) }, \ + { "chan1_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_hil_rc_inputs_raw_t, chan1_raw) }, \ + { "chan2_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_hil_rc_inputs_raw_t, chan2_raw) }, \ + { "chan3_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_hil_rc_inputs_raw_t, chan3_raw) }, \ + { "chan4_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_hil_rc_inputs_raw_t, chan4_raw) }, \ + { "chan5_raw", MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_hil_rc_inputs_raw_t, chan5_raw) }, \ + { "chan6_raw", MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_hil_rc_inputs_raw_t, chan6_raw) }, \ + { "chan7_raw", MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_rc_inputs_raw_t, chan7_raw) }, \ + { "chan8_raw", MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_rc_inputs_raw_t, chan8_raw) }, \ + { "chan9_raw", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_rc_inputs_raw_t, chan9_raw) }, \ + { "chan10_raw", MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_hil_rc_inputs_raw_t, chan10_raw) }, \ + { "chan11_raw", MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_hil_rc_inputs_raw_t, chan11_raw) }, \ + { "chan12_raw", MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_hil_rc_inputs_raw_t, chan12_raw) }, \ + { "rssi", MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_hil_rc_inputs_raw_t, rssi) }, \ + } \ +} + + /** * @brief Pack a hil_rc_inputs_raw message * @param system_id ID of this system @@ -47,31 +68,31 @@ typedef struct __mavlink_hil_rc_inputs_raw_t * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_us, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; - p->time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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->chan9_raw = chan9_raw; // uint16_t:RC channel 9 value, in microseconds - p->chan10_raw = chan10_raw; // uint16_t:RC channel 10 value, in microseconds - p->chan11_raw = chan11_raw; // uint16_t:RC channel 11 value, in microseconds - p->chan12_raw = chan12_raw; // uint16_t:RC channel 12 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, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_uint16_t_by_index(msg, 8, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 10, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 12, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 14, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 16, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 18, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 20, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 22, chan8_raw); // RC channel 8 value, in microseconds + put_uint16_t_by_index(msg, 24, chan9_raw); // RC channel 9 value, in microseconds + put_uint16_t_by_index(msg, 26, chan10_raw); // RC channel 10 value, in microseconds + put_uint16_t_by_index(msg, 28, chan11_raw); // RC channel 11 value, in microseconds + put_uint16_t_by_index(msg, 30, chan12_raw); // RC channel 12 value, in microseconds + put_uint8_t_by_index(msg, 32, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + return mavlink_finalize_message(msg, system_id, component_id, 33, 156); } /** - * @brief Pack a hil_rc_inputs_raw message + * @brief Pack a hil_rc_inputs_raw message on a channel * @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 @@ -92,28 +113,76 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_us,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint8_t rssi) +{ + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; + + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_uint16_t_by_index(msg, 8, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 10, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 12, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 14, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 16, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 18, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 20, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 22, chan8_raw); // RC channel 8 value, in microseconds + put_uint16_t_by_index(msg, 24, chan9_raw); // RC channel 9 value, in microseconds + put_uint16_t_by_index(msg, 26, chan10_raw); // RC channel 10 value, in microseconds + put_uint16_t_by_index(msg, 28, chan11_raw); // RC channel 11 value, in microseconds + put_uint16_t_by_index(msg, 30, chan12_raw); // RC channel 12 value, in microseconds + put_uint8_t_by_index(msg, 32, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 156); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a hil_rc_inputs_raw message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param 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 + * @param chan9_raw RC channel 9 value, in microseconds + * @param chan10_raw RC channel 10 value, in microseconds + * @param chan11_raw RC channel 11 value, in microseconds + * @param chan12_raw RC channel 12 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + */ +static inline void mavlink_msg_hil_rc_inputs_raw_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t time_us,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint8_t rssi) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; - p->time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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->chan9_raw = chan9_raw; // uint16_t:RC channel 9 value, in microseconds - p->chan10_raw = chan10_raw; // uint16_t:RC channel 10 value, in microseconds - p->chan11_raw = chan11_raw; // uint16_t:RC channel 11 value, in microseconds - p->chan12_raw = chan12_raw; // uint16_t:RC channel 12 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, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_uint16_t_by_index(msg, 8, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 10, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 12, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 14, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 16, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 18, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 20, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 22, chan8_raw); // RC channel 8 value, in microseconds + put_uint16_t_by_index(msg, 24, chan9_raw); // RC channel 9 value, in microseconds + put_uint16_t_by_index(msg, 26, chan10_raw); // RC channel 10 value, in microseconds + put_uint16_t_by_index(msg, 28, chan11_raw); // RC channel 11 value, in microseconds + put_uint16_t_by_index(msg, 30, chan12_raw); // RC channel 12 value, in microseconds + put_uint8_t_by_index(msg, 32, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + mavlink_finalize_message_chan_send(msg, chan, 33, 156); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a hil_rc_inputs_raw struct into a message @@ -128,8 +197,6 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u return mavlink_msg_hil_rc_inputs_raw_pack(system_id, component_id, msg, hil_rc_inputs_raw->time_us, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a hil_rc_inputs_raw message * @param chan MAVLink channel to send the message @@ -149,48 +216,19 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u * @param chan12_raw RC channel 12 value, in microseconds * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, uint64_t time_us, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) { - mavlink_header_t hdr; - mavlink_hil_rc_inputs_raw_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN ) - payload.time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - payload.chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - payload.chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - payload.chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - payload.chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - payload.chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - payload.chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - payload.chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - payload.chan9_raw = chan9_raw; // uint16_t:RC channel 9 value, in microseconds - payload.chan10_raw = chan10_raw; // uint16_t:RC channel 10 value, in microseconds - payload.chan11_raw = chan11_raw; // uint16_t:RC channel 11 value, in microseconds - payload.chan12_raw = chan12_raw; // uint16_t:RC channel 12 value, in microseconds - payload.rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN; - hdr.msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x3B, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 33); + mavlink_msg_hil_rc_inputs_raw_pack_chan_send(chan, msg, time_us, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi); } #endif + // MESSAGE HIL_RC_INPUTS_RAW UNPACKING + /** * @brief Get field time_us from hil_rc_inputs_raw message * @@ -198,8 +236,7 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui */ static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_us(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint64_t)(p->time_us); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -209,8 +246,7 @@ static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_us(const mavlink_m */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan1_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -220,8 +256,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan2_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 10); } /** @@ -231,8 +266,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan3_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -242,8 +276,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan4_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 14); } /** @@ -253,8 +286,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan5_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 16); } /** @@ -264,8 +296,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan6_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 18); } /** @@ -275,8 +306,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan7_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 20); } /** @@ -286,8 +316,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan8_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 22); } /** @@ -297,8 +326,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan9_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 24); } /** @@ -308,8 +336,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan10_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 26); } /** @@ -319,8 +346,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlin */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan11_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 28); } /** @@ -330,8 +356,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlin */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan12_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 30); } /** @@ -341,8 +366,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlin */ static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_message_t* msg) { - mavlink_hil_rc_inputs_raw_t *p = (mavlink_hil_rc_inputs_raw_t *)&msg->payload[0]; - return (uint8_t)(p->rssi); + return MAVLINK_MSG_RETURN_uint8_t(msg, 32); } /** @@ -353,5 +377,22 @@ static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_messa */ static inline void mavlink_msg_hil_rc_inputs_raw_decode(const mavlink_message_t* msg, mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) { - memcpy( hil_rc_inputs_raw, msg->payload, sizeof(mavlink_hil_rc_inputs_raw_t)); +#if MAVLINK_NEED_BYTE_SWAP + hil_rc_inputs_raw->time_us = mavlink_msg_hil_rc_inputs_raw_get_time_us(msg); + hil_rc_inputs_raw->chan1_raw = mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(msg); + hil_rc_inputs_raw->chan2_raw = mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(msg); + hil_rc_inputs_raw->chan3_raw = mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(msg); + hil_rc_inputs_raw->chan4_raw = mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(msg); + hil_rc_inputs_raw->chan5_raw = mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(msg); + hil_rc_inputs_raw->chan6_raw = mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(msg); + hil_rc_inputs_raw->chan7_raw = mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(msg); + hil_rc_inputs_raw->chan8_raw = mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(msg); + hil_rc_inputs_raw->chan9_raw = mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(msg); + hil_rc_inputs_raw->chan10_raw = mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(msg); + hil_rc_inputs_raw->chan11_raw = mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(msg); + hil_rc_inputs_raw->chan12_raw = mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(msg); + hil_rc_inputs_raw->rssi = mavlink_msg_hil_rc_inputs_raw_get_rssi(msg); +#else + memcpy(hil_rc_inputs_raw, MAVLINK_PAYLOAD(msg), 33); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h index cc172725ef93e0f942833063c12d57e3f7f84298..baae6d4c943b8b7009d15390f807619a51f4dd94 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h @@ -1,32 +1,55 @@ // MESSAGE HIL_STATE PACKING #define MAVLINK_MSG_ID_HIL_STATE 90 -#define MAVLINK_MSG_ID_HIL_STATE_LEN 56 -#define MAVLINK_MSG_90_LEN 56 -#define MAVLINK_MSG_ID_HIL_STATE_KEY 0x12 -#define MAVLINK_MSG_90_KEY 0x12 -typedef struct __mavlink_hil_state_t +typedef struct __mavlink_hil_state_t { - uint64_t time_us; ///< 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) - + uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) } mavlink_hil_state_t; +#define MAVLINK_MSG_ID_HIL_STATE_LEN 56 +#define MAVLINK_MSG_ID_90_LEN 56 + + + +#define MAVLINK_MESSAGE_INFO_HIL_STATE { \ + "HIL_STATE", \ + 16, \ + { { "time_us", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, time_us) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \ + { "rollspeed", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \ + { "pitchspeed", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \ + { "yawspeed", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \ + { "lat", MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \ + { "lon", MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \ + { "alt", MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \ + { "vx", MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \ + { "vy", MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \ + { "vz", MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \ + { "xacc", MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \ + { "yacc", MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \ + { "zacc", MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \ + } \ +} + + /** * @brief Pack a hil_state message * @param system_id ID of this system @@ -51,33 +74,33 @@ typedef struct __mavlink_hil_state_t * @param zacc Z acceleration (mg) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, 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) +static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_us, 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_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - p->time_us = time_us; // 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_HIL_STATE_LEN); + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll); // Roll angle (rad) + put_float_by_index(msg, 12, pitch); // Pitch angle (rad) + put_float_by_index(msg, 16, yaw); // Yaw angle (rad) + put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) + put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) + put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) + put_int32_t_by_index(msg, 32, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 36, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 40, alt); // Altitude in meters, expressed as * 1000 (millimeters) + put_int16_t_by_index(msg, 44, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 46, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 48, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 50, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 52, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 54, zacc); // Z acceleration (mg) + + return mavlink_finalize_message(msg, system_id, component_id, 56, 12); } /** - * @brief Pack a hil_state message + * @brief Pack a hil_state message on a channel * @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 @@ -100,30 +123,82 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com * @param zacc Z acceleration (mg) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, 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) +static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_us,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) +{ + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; + + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll); // Roll angle (rad) + put_float_by_index(msg, 12, pitch); // Pitch angle (rad) + put_float_by_index(msg, 16, yaw); // Yaw angle (rad) + put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) + put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) + put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) + put_int32_t_by_index(msg, 32, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 36, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 40, alt); // Altitude in meters, expressed as * 1000 (millimeters) + put_int16_t_by_index(msg, 44, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 46, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 48, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 50, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 52, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 54, zacc); // Z acceleration (mg) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 56, 12); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a hil_state message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll 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) + */ +static inline void mavlink_msg_hil_state_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t time_us,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_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - p->time_us = time_us; // 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_HIL_STATE_LEN); + put_uint64_t_by_index(msg, 0, time_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, roll); // Roll angle (rad) + put_float_by_index(msg, 12, pitch); // Pitch angle (rad) + put_float_by_index(msg, 16, yaw); // Yaw angle (rad) + put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) + put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) + put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) + put_int32_t_by_index(msg, 32, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 36, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 40, alt); // Altitude in meters, expressed as * 1000 (millimeters) + put_int16_t_by_index(msg, 44, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 46, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 48, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 50, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 52, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 54, zacc); // Z acceleration (mg) + + mavlink_finalize_message_chan_send(msg, chan, 56, 12); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a hil_state struct into a message @@ -138,8 +213,6 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_us, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a hil_state message * @param chan MAVLink channel to send the message @@ -161,50 +234,19 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c * @param yacc Y acceleration (mg) * @param zacc Z acceleration (mg) */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_us, 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_hil_state_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HIL_STATE_LEN ) - payload.time_us = time_us; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.roll = roll; // float:Roll angle (rad) - payload.pitch = pitch; // float:Pitch angle (rad) - payload.yaw = yaw; // float:Yaw angle (rad) - payload.rollspeed = rollspeed; // float:Roll angular speed (rad/s) - payload.pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - payload.yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - payload.lat = lat; // int32_t:Latitude, expressed as * 1E7 - payload.lon = lon; // int32_t:Longitude, expressed as * 1E7 - payload.alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - payload.vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - payload.vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - payload.vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - payload.xacc = xacc; // int16_t:X acceleration (mg) - payload.yacc = yacc; // int16_t:Y acceleration (mg) - payload.zacc = zacc; // int16_t:Z acceleration (mg) - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_HIL_STATE_LEN; - hdr.msgid = MAVLINK_MSG_ID_HIL_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x12, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 56); + mavlink_msg_hil_state_pack_chan_send(chan, msg, time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); } #endif + // MESSAGE HIL_STATE UNPACKING + /** * @brief Get field time_us from hil_state message * @@ -212,8 +254,7 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t */ static inline uint64_t mavlink_msg_hil_state_get_time_us(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (uint64_t)(p->time_us); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -223,8 +264,7 @@ static inline uint64_t mavlink_msg_hil_state_get_time_us(const mavlink_message_t */ static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -234,8 +274,7 @@ static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -245,8 +284,7 @@ static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg */ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -256,8 +294,7 @@ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (float)(p->rollspeed); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -267,8 +304,7 @@ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (float)(p->pitchspeed); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -278,8 +314,7 @@ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t */ static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (float)(p->yawspeed); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -289,8 +324,7 @@ static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* */ static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int32_t)(p->lat); + return MAVLINK_MSG_RETURN_int32_t(msg, 32); } /** @@ -300,8 +334,7 @@ static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg */ static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int32_t)(p->lon); + return MAVLINK_MSG_RETURN_int32_t(msg, 36); } /** @@ -311,8 +344,7 @@ static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg */ static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int32_t)(p->alt); + return MAVLINK_MSG_RETURN_int32_t(msg, 40); } /** @@ -322,8 +354,7 @@ static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int16_t)(p->vx); + return MAVLINK_MSG_RETURN_int16_t(msg, 44); } /** @@ -333,8 +364,7 @@ static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int16_t)(p->vy); + return MAVLINK_MSG_RETURN_int16_t(msg, 46); } /** @@ -344,8 +374,7 @@ static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int16_t)(p->vz); + return MAVLINK_MSG_RETURN_int16_t(msg, 48); } /** @@ -355,8 +384,7 @@ static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int16_t)(p->xacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 50); } /** @@ -366,8 +394,7 @@ static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* ms */ static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int16_t)(p->yacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 52); } /** @@ -377,8 +404,7 @@ static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* ms */ static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) { - mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0]; - return (int16_t)(p->zacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 54); } /** @@ -389,5 +415,24 @@ static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* ms */ static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state) { - memcpy( hil_state, msg->payload, sizeof(mavlink_hil_state_t)); +#if MAVLINK_NEED_BYTE_SWAP + hil_state->time_us = mavlink_msg_hil_state_get_time_us(msg); + hil_state->roll = mavlink_msg_hil_state_get_roll(msg); + hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg); + hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg); + hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg); + hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg); + hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg); + hil_state->lat = mavlink_msg_hil_state_get_lat(msg); + hil_state->lon = mavlink_msg_hil_state_get_lon(msg); + hil_state->alt = mavlink_msg_hil_state_get_alt(msg); + hil_state->vx = mavlink_msg_hil_state_get_vx(msg); + hil_state->vy = mavlink_msg_hil_state_get_vy(msg); + hil_state->vz = mavlink_msg_hil_state_get_vz(msg); + hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg); + hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg); + hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg); +#else + memcpy(hil_state, MAVLINK_PAYLOAD(msg), 56); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index 23bef43c77c8bdd8788a62336e1ee4ce475349f4..69f688d024735e7834f496d9de0d17776c3279f3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_LOCAL_POSITION_KEY 0xF0 -#define MAVLINK_MSG_31_KEY 0xF0 -typedef struct __mavlink_local_position_t +typedef struct __mavlink_local_position_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - float vx; ///< X Speed - float vy; ///< Y Speed - float vz; ///< Z Speed - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + float vx; ///< X Speed + float vy; ///< Y Speed + float vz; ///< Z Speed } mavlink_local_position_t; +#define MAVLINK_MSG_ID_LOCAL_POSITION_LEN 32 +#define MAVLINK_MSG_ID_31_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION { \ + "LOCAL_POSITION", \ + 7, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_t, z) }, \ + { "vx", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_t, vx) }, \ + { "vy", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_t, vy) }, \ + { "vz", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_t, vz) }, \ + } \ +} + + /** * @brief Pack a local_position message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_local_position_t * @param vz Z Speed * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, x); // X Position + put_float_by_index(msg, 12, y); // Y Position + put_float_by_index(msg, 16, z); // Z Position + put_float_by_index(msg, 20, vx); // X Speed + put_float_by_index(msg, 24, vy); // Y Speed + put_float_by_index(msg, 28, vz); // Z Speed - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 126); } /** - * @brief Pack a local_position message + * @brief Pack a local_position message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ * @param vz Z Speed * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, x); // X Position + put_float_by_index(msg, 12, y); // Y Position + put_float_by_index(msg, 16, z); // Z Position + put_float_by_index(msg, 20, vx); // X Speed + put_float_by_index(msg, 24, vy); // Y Speed + put_float_by_index(msg, 28, vz); // Z Speed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 126); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a local_position message on a channel and send + * @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 x X Position + * @param y Y Position + * @param z Z Position + * @param vx X Speed + * @param vy Y Speed + * @param vz Z Speed + */ +static inline void mavlink_msg_local_position_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z,float vx,float vy,float vz) +{ + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, x); // X Position + put_float_by_index(msg, 12, y); // Y Position + put_float_by_index(msg, 16, z); // Z Position + put_float_by_index(msg, 20, vx); // X Speed + put_float_by_index(msg, 24, vy); // Y Speed + put_float_by_index(msg, 28, vz); // Z Speed + + mavlink_finalize_message_chan_send(msg, chan, 32, 126); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a local_position struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint return mavlink_msg_local_position_pack(system_id, component_id, msg, local_position->usec, local_position->x, local_position->y, local_position->z, local_position->vx, local_position->vy, local_position->vz); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a local_position message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_local_position_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.x = x; // float:X Position - payload.y = y; // float:Y Position - payload.z = z; // float:Z Position - payload.vx = vx; // float:X Speed - payload.vy = vy; // float:Y Speed - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF0, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_local_position_pack_chan_send(chan, msg, usec, x, y, z, vx, vy, vz); } #endif + // MESSAGE LOCAL_POSITION UNPACKING + /** * @brief Get field usec from local_position message * @@ -149,8 +173,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (float)(p->vx); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (float)(p->vy); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; - return (float)(p->vz); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( local_position, msg->payload, sizeof(mavlink_local_position_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(local_position, MAVLINK_PAYLOAD(msg), 32); +#endif } 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 8946df5040d8c5f5558cb177e5312cba4363dff5..814f13b69dd1032ec827b81267d2402ed8b2cab9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -1,20 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_KEY 0x4B -#define MAVLINK_MSG_51_KEY 0x4B -typedef struct __mavlink_local_position_setpoint_t +typedef struct __mavlink_local_position_setpoint_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< Desired yaw angle } mavlink_local_position_setpoint_t; +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 16 +#define MAVLINK_MSG_ID_51_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT { \ + "LOCAL_POSITION_SETPOINT", \ + 4, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_t, yaw) }, \ + } \ +} + + /** * @brief Pack a local_position_setpoint message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_local_position_setpoint_t * @param yaw Desired yaw angle * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 16, 50); } /** - * @brief Pack a local_position_setpoint message + * @brief Pack a local_position_setpoint message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i * @param yaw Desired yaw angle * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // Desired yaw angle + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 50); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a local_position_setpoint message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param x x position + * @param y y position + * @param z z position + * @param yaw Desired yaw angle + */ +static inline void mavlink_msg_local_position_setpoint_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float x,float y,float z,float yaw) { - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // Desired yaw angle - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 16, 50); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a local_position_setpoint struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a local_position_setpoint message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - mavlink_header_t hdr; - mavlink_local_position_setpoint_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN ) - payload.x = x; // float:x position - payload.y = y; // float:y position - payload.z = z; // float:z position - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x4B, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 16); + mavlink_msg_local_position_setpoint_pack_chan_send(chan, msg, x, y, z, yaw); } #endif + // MESSAGE LOCAL_POSITION_SETPOINT UNPACKING + /** * @brief Get field x from local_position_setpoint message * @@ -128,8 +146,7 @@ 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) { - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( local_position_setpoint, msg->payload, sizeof(mavlink_local_position_setpoint_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(local_position_setpoint, MAVLINK_PAYLOAD(msg), 16); +#endif } 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 2cc141437272d824aaadb6352fd5aec2aaa760ce..15a56ee663780539c0ecf9f3d71128396f4984ba 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,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_KEY 0xA -#define MAVLINK_MSG_50_KEY 0xA -typedef struct __mavlink_local_position_setpoint_set_t +typedef struct __mavlink_local_position_setpoint_set_t { - 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 - + 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; +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN 18 +#define MAVLINK_MSG_ID_50_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET { \ + "LOCAL_POSITION_SETPOINT_SET", \ + 6, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_set_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_set_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_set_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_set_t, yaw) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_local_position_setpoint_set_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_local_position_setpoint_set_t, target_component) }, \ + } \ +} + + /** * @brief Pack a local_position_setpoint_set message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_local_position_setpoint_set_t * @param yaw Desired yaw angle * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // Desired yaw angle + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 73); } /** - * @brief Pack a local_position_setpoint_set message + * @brief Pack a local_position_setpoint_set message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst * @param yaw Desired yaw angle * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // Desired yaw angle + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 73); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a local_position_setpoint_set message on a channel and send + * @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 x x position + * @param y y position + * @param z z position + * @param yaw Desired yaw angle + */ +static inline void mavlink_msg_local_position_setpoint_set_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float x,float y,float z,float yaw) { - 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; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // Desired yaw angle + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 73); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a local_position_setpoint_set struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy return mavlink_msg_local_position_setpoint_set_pack(system_id, component_id, msg, local_position_setpoint_set->target_system, local_position_setpoint_set->target_component, local_position_setpoint_set->x, local_position_setpoint_set->y, local_position_setpoint_set->z, local_position_setpoint_set->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a local_position_setpoint_set message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_local_position_setpoint_set_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.x = x; // float:x position - payload.y = y; // float:y position - payload.z = z; // float:z position - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_local_position_setpoint_set_pack_chan_send(chan, msg, target_system, target_component, x, y, z, yaw); } #endif + // MESSAGE LOCAL_POSITION_SETPOINT_SET UNPACKING + /** * @brief Get field target_system from local_position_setpoint_set message * @@ -142,8 +164,7 @@ 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) { - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( local_position_setpoint_set, msg->payload, sizeof(mavlink_local_position_setpoint_set_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + 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); +#else + memcpy(local_position_setpoint_set, MAVLINK_PAYLOAD(msg), 18); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index 30fe96a873d5807becaa70115194badd70d71bac..7f4a255507c499edc1e15c463db6340f5bdcae2d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -1,25 +1,41 @@ // 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 -#define MAVLINK_MSG_ID_MANUAL_CONTROL_KEY 0x7F -#define MAVLINK_MSG_69_KEY 0x7F -typedef struct __mavlink_manual_control_t +typedef struct __mavlink_manual_control_t { - 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 - uint8_t thrust_manual; ///< thrust auto:0, manual:1 - + 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 + uint8_t thrust_manual; ///< thrust auto:0, manual:1 } mavlink_manual_control_t; +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 21 +#define MAVLINK_MSG_ID_69_LEN 21 + + + +#define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \ + "MANUAL_CONTROL", \ + 9, \ + { { "roll", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_manual_control_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_manual_control_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_manual_control_t, yaw) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_manual_control_t, thrust) }, \ + { "target", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_manual_control_t, target) }, \ + { "roll_manual", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_manual_control_t, roll_manual) }, \ + { "pitch_manual", MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_manual_control_t, pitch_manual) }, \ + { "yaw_manual", MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_manual_control_t, yaw_manual) }, \ + { "thrust_manual", MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_manual_control_t, thrust_manual) }, \ + } \ +} + + /** * @brief Pack a manual_control message * @param system_id ID of this system @@ -37,26 +53,26 @@ typedef struct __mavlink_manual_control_t * @param thrust_manual thrust auto:0, manual:1 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - 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, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); + put_float_by_index(msg, 0, roll); // roll + put_float_by_index(msg, 4, pitch); // pitch + put_float_by_index(msg, 8, yaw); // yaw + put_float_by_index(msg, 12, thrust); // thrust + put_uint8_t_by_index(msg, 16, target); // The system to be controlled + put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 + put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 + put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 + put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + + return mavlink_finalize_message(msg, system_id, component_id, 21, 52); } /** - * @brief Pack a manual_control message + * @brief Pack a manual_control message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ * @param thrust_manual thrust auto:0, manual:1 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - 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, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); + put_float_by_index(msg, 0, roll); // roll + put_float_by_index(msg, 4, pitch); // pitch + put_float_by_index(msg, 8, yaw); // yaw + put_float_by_index(msg, 12, thrust); // thrust + put_uint8_t_by_index(msg, 16, target); // The system to be controlled + put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 + put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 + put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 + put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 52); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a manual_control message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system to be controlled + * @param roll roll + * @param pitch pitch + * @param yaw yaw + * @param thrust thrust + * @param roll_manual roll control enabled auto:0, manual:1 + * @param pitch_manual pitch auto:0, manual:1 + * @param yaw_manual yaw auto:0, manual:1 + * @param thrust_manual thrust auto:0, manual:1 + */ +static inline void mavlink_msg_manual_control_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + + put_float_by_index(msg, 0, roll); // roll + put_float_by_index(msg, 4, pitch); // pitch + put_float_by_index(msg, 8, yaw); // yaw + put_float_by_index(msg, 12, thrust); // thrust + put_uint8_t_by_index(msg, 16, target); // The system to be controlled + put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 + put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 + put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 + put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + + mavlink_finalize_message_chan_send(msg, chan, 21, 52); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a manual_control struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->roll, manual_control->pitch, manual_control->yaw, manual_control->thrust, manual_control->roll_manual, manual_control->pitch_manual, manual_control->yaw_manual, manual_control->thrust_manual); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a manual_control message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_manual_control_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN ) - payload.target = target; // uint8_t:The system to be controlled - payload.roll = roll; // float:roll - payload.pitch = pitch; // float:pitch - payload.yaw = yaw; // float:yaw - payload.thrust = thrust; // float:thrust - payload.roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - payload.pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - payload.yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x7F, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 21); + mavlink_msg_manual_control_pack_chan_send(chan, msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); } #endif + // MESSAGE MANUAL_CONTROL UNPACKING + /** * @brief Get field target from manual_control message * @@ -163,8 +191,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (float)(p->thrust); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (uint8_t)(p->roll_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (uint8_t)(p->pitch_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 18); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (uint8_t)(p->yaw_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 19); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; - return (uint8_t)(p->thrust_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 20); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( manual_control, msg->payload, sizeof(mavlink_manual_control_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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->target = mavlink_msg_manual_control_get_target(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); +#else + memcpy(manual_control, MAVLINK_PAYLOAD(msg), 21); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h index 13f7e61149fa372e27dcd1dec45998a626e61cfd..b4b32407327aab9cebf72569cba6e064b1dc055d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -1,21 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_MEMORY_VECT_KEY 0x8A -#define MAVLINK_MSG_250_KEY 0x8A -typedef struct __mavlink_memory_vect_t +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 - + 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_ID_MEMORY_VECT_LEN 36 +#define MAVLINK_MSG_ID_250_LEN 36 + #define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32 +#define MAVLINK_MESSAGE_INFO_MEMORY_VECT { \ + "MEMORY_VECT", \ + 4, \ + { { "address", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_memory_vect_t, address) }, \ + { "ver", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_memory_vect_t, ver) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_memory_vect_t, type) }, \ + { "value", MAVLINK_TYPE_INT8_T, 32, 4, offsetof(mavlink_memory_vect_t, value) }, \ + } \ +} + + /** * @brief Pack a memory_vect message * @param system_id ID of this system @@ -28,21 +38,21 @@ typedef struct __mavlink_memory_vect_t * @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) +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 + put_uint16_t_by_index(msg, 0, address); // Starting address of the debug variables + put_uint8_t_by_index(msg, 2, ver); // Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + put_uint8_t_by_index(msg, 3, 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 + put_int8_t_array_by_index(msg, 4, value, 32); // Memory contents at specified address - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 36, 204); } /** - * @brief Pack a memory_vect message + * @brief Pack a memory_vect message on a channel * @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 @@ -53,18 +63,46 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c * @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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; + + put_uint16_t_by_index(msg, 0, address); // Starting address of the debug variables + put_uint8_t_by_index(msg, 2, ver); // Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + put_uint8_t_by_index(msg, 3, 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 + put_int8_t_array_by_index(msg, 4, value, 32); // Memory contents at specified address + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 204); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a memory_vect message on a channel and send + * @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 + */ +static inline void mavlink_msg_memory_vect_pack_chan_send(mavlink_channel_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 + put_uint16_t_by_index(msg, 0, address); // Starting address of the debug variables + put_uint8_t_by_index(msg, 2, ver); // Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + put_uint8_t_by_index(msg, 3, 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 + put_int8_t_array_by_index(msg, 4, value, 32); // Memory contents at specified address - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 36, 204); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a memory_vect struct into a message @@ -79,8 +117,6 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a memory_vect message * @param chan MAVLink channel to send the message @@ -90,38 +126,19 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t * @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 */ -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) +#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; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN ) - payload.address = address; // uint16_t:Starting address of the debug variables - payload.ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - payload.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(payload.value, value, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x8A, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 36); + mavlink_msg_memory_vect_pack_chan_send(chan, msg, address, ver, type, value); } #endif + // MESSAGE MEMORY_VECT UNPACKING + /** * @brief Get field address from memory_vect message * @@ -129,8 +146,7 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -140,8 +156,7 @@ static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message */ 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); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -151,8 +166,7 @@ static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* m */ 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); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -160,12 +174,9 @@ static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* * * @return Memory contents at specified address */ -static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t* value) +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); + return MAVLINK_MSG_RETURN_int8_t_array(msg, value, 32, 4); } /** @@ -176,5 +187,12 @@ static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t */ 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)); +#if MAVLINK_NEED_BYTE_SWAP + memory_vect->address = mavlink_msg_memory_vect_get_address(msg); + memory_vect->ver = mavlink_msg_memory_vect_get_ver(msg); + memory_vect->type = mavlink_msg_memory_vect_get_type(msg); + mavlink_msg_memory_vect_get_value(msg, memory_vect->value); +#else + memcpy(memory_vect, MAVLINK_PAYLOAD(msg), 36); +#endif } 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 2b5aa2a11b04bb94f819a7c83f4a0f624d839165..6ffb734caed7026cfc7d982858bb8038c107faad 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -1,19 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_KEY 0x8D -#define MAVLINK_MSG_252_KEY 0x8D -typedef struct __mavlink_named_value_float_t +typedef struct __mavlink_named_value_float_t { - float value; ///< Floating point value - 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_ID_NAMED_VALUE_FLOAT_LEN 14 +#define MAVLINK_MSG_ID_252_LEN 14 + #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 +#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT { \ + "NAMED_VALUE_FLOAT", \ + 2, \ + { { "value", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_named_value_float_t, value) }, \ + { "name", MAVLINK_TYPE_CHAR, 10, 4, offsetof(mavlink_named_value_float_t, name) }, \ + } \ +} + + /** * @brief Pack a named_value_float message * @param system_id ID of this system @@ -24,19 +32,19 @@ typedef struct __mavlink_named_value_float_t * @param value Floating point value * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value + put_float_by_index(msg, 0, value); // Floating point value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 14, 248); } /** - * @brief Pack a named_value_float message + * @brief Pack a named_value_float message on a channel * @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 @@ -45,16 +53,40 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin * @param value Floating point value * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + + put_float_by_index(msg, 0, value); // Floating point value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 248); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a named_value_float message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param name Name of the debug variable + * @param value Floating point value + */ +static inline void mavlink_msg_named_value_float_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + const char *name,float value) { - mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value + put_float_by_index(msg, 0, value); // Floating point value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 14, 248); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a named_value_float struct into a message @@ -69,8 +101,6 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->name, named_value_float->value); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a named_value_float message * @param chan MAVLink channel to send the message @@ -78,47 +108,27 @@ 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 */ -static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char *name, float value) { - mavlink_header_t hdr; - mavlink_named_value_float_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN ) - memcpy(payload.name, name, sizeof(payload.name)); // char[10]:Name of the debug variable - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x8D, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 14); + mavlink_msg_named_value_float_pack_chan_send(chan, msg, name, value); } #endif + // MESSAGE NAMED_VALUE_FLOAT UNPACKING + /** * @brief Get field name from named_value_float message * * @return Name of the debug variable */ -static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char* name) +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(name, p->name, sizeof(p->name)); - return sizeof(p->name); + return MAVLINK_MSG_RETURN_char_array(msg, name, 10, 4); } /** @@ -128,8 +138,7 @@ 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) { - mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; - return (float)(p->value); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -140,5 +149,10 @@ 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) { - memcpy( named_value_float, msg->payload, sizeof(mavlink_named_value_float_t)); +#if MAVLINK_NEED_BYTE_SWAP + named_value_float->value = mavlink_msg_named_value_float_get_value(msg); + mavlink_msg_named_value_float_get_name(msg, named_value_float->name); +#else + memcpy(named_value_float, MAVLINK_PAYLOAD(msg), 14); +#endif } 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 ec212acaa8b5b2c416db6beb40ef19e8aeadba74..41210e84322e1f6ddc16267f7944b5961e9dbbce 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -1,19 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_NAMED_VALUE_INT_KEY 0xD3 -#define MAVLINK_MSG_253_KEY 0xD3 -typedef struct __mavlink_named_value_int_t +typedef struct __mavlink_named_value_int_t { - int32_t value; ///< Signed integer value - 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_ID_NAMED_VALUE_INT_LEN 14 +#define MAVLINK_MSG_ID_253_LEN 14 + #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 +#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT { \ + "NAMED_VALUE_INT", \ + 2, \ + { { "value", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_named_value_int_t, value) }, \ + { "name", MAVLINK_TYPE_CHAR, 10, 4, offsetof(mavlink_named_value_int_t, name) }, \ + } \ +} + + /** * @brief Pack a named_value_int message * @param system_id ID of this system @@ -24,19 +32,19 @@ typedef struct __mavlink_named_value_int_t * @param value Signed integer value * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value + put_int32_t_by_index(msg, 0, value); // Signed integer value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 14, 63); } /** - * @brief Pack a named_value_int message + * @brief Pack a named_value_int message on a channel * @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 @@ -45,16 +53,40 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 * @param value Signed integer value * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + + put_int32_t_by_index(msg, 0, value); // Signed integer value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 63); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a named_value_int message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param name Name of the debug variable + * @param value Signed integer value + */ +static inline void mavlink_msg_named_value_int_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + const char *name,int32_t value) { - mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value + put_int32_t_by_index(msg, 0, value); // Signed integer value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 14, 63); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a named_value_int struct into a message @@ -69,8 +101,6 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->name, named_value_int->value); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a named_value_int message * @param chan MAVLink channel to send the message @@ -78,47 +108,27 @@ 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 */ -static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) +#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_header_t hdr; - mavlink_named_value_int_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN ) - memcpy(payload.name, name, sizeof(payload.name)); // char[10]:Name of the debug variable - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xD3, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 14); + mavlink_msg_named_value_int_pack_chan_send(chan, msg, name, value); } #endif + // MESSAGE NAMED_VALUE_INT UNPACKING + /** * @brief Get field name from named_value_int message * * @return Name of the debug variable */ -static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char* name) +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(name, p->name, sizeof(p->name)); - return sizeof(p->name); + return MAVLINK_MSG_RETURN_char_array(msg, name, 10, 4); } /** @@ -128,8 +138,7 @@ 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) { - mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; - return (int32_t)(p->value); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -140,5 +149,10 @@ 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) { - memcpy( named_value_int, msg->payload, sizeof(mavlink_named_value_int_t)); +#if MAVLINK_NEED_BYTE_SWAP + named_value_int->value = mavlink_msg_named_value_int_get_value(msg); + mavlink_msg_named_value_int_get_name(msg, named_value_int->name); +#else + memcpy(named_value_int, MAVLINK_PAYLOAD(msg), 14); +#endif } 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 1bb5c22cf9fe3c8c8856b821fa2a0b800713fa8b..659f1a162b33bc7df0c38b5f006db6c1e0068f0b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -1,24 +1,39 @@ // 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 -#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_KEY 0x3E -#define MAVLINK_MSG_62_KEY 0x3E -typedef struct __mavlink_nav_controller_output_t +typedef struct __mavlink_nav_controller_output_t { - float nav_roll; ///< Current desired roll in degrees - float nav_pitch; ///< Current desired pitch in degrees - 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 - + float nav_roll; ///< Current desired roll in degrees + float nav_pitch; ///< Current desired pitch in degrees + 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; +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 +#define MAVLINK_MSG_ID_62_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT { \ + "NAV_CONTROLLER_OUTPUT", \ + 8, \ + { { "nav_roll", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_nav_controller_output_t, nav_roll) }, \ + { "nav_pitch", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_nav_controller_output_t, nav_pitch) }, \ + { "alt_error", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_nav_controller_output_t, alt_error) }, \ + { "aspd_error", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_nav_controller_output_t, aspd_error) }, \ + { "xtrack_error", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_nav_controller_output_t, xtrack_error) }, \ + { "nav_bearing", MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_nav_controller_output_t, nav_bearing) }, \ + { "target_bearing", MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_nav_controller_output_t, target_bearing) }, \ + { "wp_dist", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_nav_controller_output_t, wp_dist) }, \ + } \ +} + + /** * @brief Pack a nav_controller_output message * @param system_id ID of this system @@ -35,25 +50,25 @@ typedef struct __mavlink_nav_controller_output_t * @param xtrack_error Current crosstrack error on x-y plane in meters * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - 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 + put_float_by_index(msg, 0, nav_roll); // Current desired roll in degrees + put_float_by_index(msg, 4, nav_pitch); // Current desired pitch in degrees + put_float_by_index(msg, 8, alt_error); // Current altitude error in meters + put_float_by_index(msg, 12, aspd_error); // Current airspeed error in meters/second + put_float_by_index(msg, 16, xtrack_error); // Current crosstrack error on x-y plane in meters + put_int16_t_by_index(msg, 20, nav_bearing); // Current desired heading in degrees + put_int16_t_by_index(msg, 22, target_bearing); // Bearing to current waypoint/target in degrees + put_uint16_t_by_index(msg, 24, wp_dist); // Distance to active waypoint in meters - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 26, 183); } /** - * @brief Pack a nav_controller_output message + * @brief Pack a nav_controller_output message on a channel * @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 @@ -68,22 +83,58 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, * @param xtrack_error Current crosstrack error on x-y plane in meters * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + + put_float_by_index(msg, 0, nav_roll); // Current desired roll in degrees + put_float_by_index(msg, 4, nav_pitch); // Current desired pitch in degrees + put_float_by_index(msg, 8, alt_error); // Current altitude error in meters + put_float_by_index(msg, 12, aspd_error); // Current airspeed error in meters/second + put_float_by_index(msg, 16, xtrack_error); // Current crosstrack error on x-y plane in meters + put_int16_t_by_index(msg, 20, nav_bearing); // Current desired heading in degrees + put_int16_t_by_index(msg, 22, target_bearing); // Bearing to current waypoint/target in degrees + put_uint16_t_by_index(msg, 24, wp_dist); // Distance to active waypoint in meters + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 183); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a nav_controller_output message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + */ +static inline void mavlink_msg_nav_controller_output_pack_chan_send(mavlink_channel_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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - 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 + put_float_by_index(msg, 0, nav_roll); // Current desired roll in degrees + put_float_by_index(msg, 4, nav_pitch); // Current desired pitch in degrees + put_float_by_index(msg, 8, alt_error); // Current altitude error in meters + put_float_by_index(msg, 12, aspd_error); // Current airspeed error in meters/second + put_float_by_index(msg, 16, xtrack_error); // Current crosstrack error on x-y plane in meters + put_int16_t_by_index(msg, 20, nav_bearing); // Current desired heading in degrees + put_int16_t_by_index(msg, 22, target_bearing); // Bearing to current waypoint/target in degrees + put_uint16_t_by_index(msg, 24, wp_dist); // Distance to active waypoint in meters - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 26, 183); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a nav_controller_output struct into a message @@ -98,8 +149,6 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a nav_controller_output message * @param chan MAVLink channel to send the message @@ -113,42 +162,19 @@ 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 + 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_header_t hdr; - mavlink_nav_controller_output_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN ) - payload.nav_roll = nav_roll; // float:Current desired roll in degrees - payload.nav_pitch = nav_pitch; // float:Current desired pitch in degrees - payload.nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees - payload.target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees - payload.wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters - payload.alt_error = alt_error; // float:Current altitude error in meters - payload.aspd_error = aspd_error; // float:Current airspeed error in meters/second - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x3E, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 26); + mavlink_msg_nav_controller_output_pack_chan_send(chan, msg, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error); } #endif + // MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING + /** * @brief Get field nav_roll from nav_controller_output message * @@ -156,8 +182,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (float)(p->nav_roll); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -167,8 +192,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (float)(p->nav_pitch); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -178,8 +202,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (int16_t)(p->nav_bearing); + return MAVLINK_MSG_RETURN_int16_t(msg, 20); } /** @@ -189,8 +212,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (int16_t)(p->target_bearing); + return MAVLINK_MSG_RETURN_int16_t(msg, 22); } /** @@ -200,8 +222,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (uint16_t)(p->wp_dist); + return MAVLINK_MSG_RETURN_uint16_t(msg, 24); } /** @@ -211,8 +232,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (float)(p->alt_error); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -222,8 +242,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (float)(p->aspd_error); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -233,8 +252,7 @@ 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) { - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; - return (float)(p->xtrack_error); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -245,5 +263,16 @@ 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) { - memcpy( nav_controller_output, msg->payload, sizeof(mavlink_nav_controller_output_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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->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); + 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); +#else + memcpy(nav_controller_output, MAVLINK_PAYLOAD(msg), 26); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h b/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h new file mode 100644 index 0000000000000000000000000000000000000000..8da2e08a1f3e5aa20110e4c3dfcfe1a99084014a --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h @@ -0,0 +1,239 @@ +// MESSAGE OBJECT_DETECTION_EVENT PACKING + +#define MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT 140 + +typedef struct __mavlink_object_detection_event_t +{ + uint32_t time; ///< Timestamp in milliseconds since system boot + float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + float distance; ///< Ground distance in meters + uint16_t object_id; ///< Object ID + uint8_t type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + char name[20]; ///< Name of the object as defined by the detector + uint8_t quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence +} mavlink_object_detection_event_t; + +/** + * @brief Pack a object_detection_event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_object_detection_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time, uint16_t object_id, uint8_t type, const char name[20], uint8_t quality, float bearing, float distance) +{ + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + + put_uint32_t_by_index(time, 0, msg->payload); // Timestamp in milliseconds since system boot + put_float_by_index(bearing, 4, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + put_float_by_index(distance, 8, msg->payload); // Ground distance in meters + put_uint16_t_by_index(object_id, 12, msg->payload); // Object ID + put_uint8_t_by_index(type, 14, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + put_char_array_by_index(name, 15, 20, msg->payload); // Name of the object as defined by the detector + put_uint8_t_by_index(quality, 35, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence + + return mavlink_finalize_message(msg, system_id, component_id, 36, 90); +} + +/** + * @brief Pack a object_detection_event message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_object_detection_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time,uint16_t object_id,uint8_t type,const char name[20],uint8_t quality,float bearing,float distance) +{ + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + + put_uint32_t_by_index(time, 0, msg->payload); // Timestamp in milliseconds since system boot + put_float_by_index(bearing, 4, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + put_float_by_index(distance, 8, msg->payload); // Ground distance in meters + put_uint16_t_by_index(object_id, 12, msg->payload); // Object ID + put_uint8_t_by_index(type, 14, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + put_char_array_by_index(name, 15, 20, msg->payload); // Name of the object as defined by the detector + put_uint8_t_by_index(quality, 35, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 90); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a object_detection_event message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + */ +static inline void mavlink_msg_object_detection_event_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint32_t time,uint16_t object_id,uint8_t type,const char name[20],uint8_t quality,float bearing,float distance) +{ + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + + put_uint32_t_by_index(time, 0, msg->payload); // Timestamp in milliseconds since system boot + put_float_by_index(bearing, 4, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + put_float_by_index(distance, 8, msg->payload); // Ground distance in meters + put_uint16_t_by_index(object_id, 12, msg->payload); // Object ID + put_uint8_t_by_index(type, 14, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + put_char_array_by_index(name, 15, 20, msg->payload); // Name of the object as defined by the detector + put_uint8_t_by_index(quality, 35, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence + + mavlink_finalize_message_chan_send(msg, chan, 36, 90); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + +/** + * @brief Encode a object_detection_event struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param object_detection_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_object_detection_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_object_detection_event_t* object_detection_event) +{ + return mavlink_msg_object_detection_event_pack(system_id, component_id, msg, object_detection_event->time, object_detection_event->object_id, object_detection_event->type, object_detection_event->name, object_detection_event->quality, object_detection_event->bearing, object_detection_event->distance); +} + +/** + * @brief Send a object_detection_event message + * @param chan MAVLink channel to send the message + * + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_object_detection_event_send(mavlink_channel_t chan, uint32_t time, uint16_t object_id, uint8_t type, const char name[20], uint8_t quality, float bearing, float distance) +{ + MAVLINK_ALIGNED_MESSAGE(msg, 36); + mavlink_msg_object_detection_event_pack_chan_send(chan, msg, time, object_id, type, name, quality, bearing, distance); +} + +#endif + +// MESSAGE OBJECT_DETECTION_EVENT UNPACKING + + +/** + * @brief Get field time from object_detection_event message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_object_detection_event_get_time(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field object_id from object_detection_event message + * + * @return Object ID + */ +static inline uint16_t mavlink_msg_object_detection_event_get_object_id(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field type from object_detection_event message + * + * @return Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + */ +static inline uint8_t mavlink_msg_object_detection_event_get_type(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field name from object_detection_event message + * + * @return Name of the object as defined by the detector + */ +static inline uint16_t mavlink_msg_object_detection_event_get_name(const mavlink_message_t* msg, char *name) +{ + return MAVLINK_MSG_RETURN_char_array(msg, name, 20, 15); +} + +/** + * @brief Get field quality from object_detection_event message + * + * @return Detection quality / confidence. 0: bad, 255: maximum confidence + */ +static inline uint8_t mavlink_msg_object_detection_event_get_quality(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field bearing from object_detection_event message + * + * @return Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + */ +static inline float mavlink_msg_object_detection_event_get_bearing(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_float(msg, 4); +} + +/** + * @brief Get field distance from object_detection_event message + * + * @return Ground distance in meters + */ +static inline float mavlink_msg_object_detection_event_get_distance(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_float(msg, 8); +} + +/** + * @brief Decode a object_detection_event message into a struct + * + * @param msg The message to decode + * @param object_detection_event C-struct to decode the message contents into + */ +static inline void mavlink_msg_object_detection_event_decode(const mavlink_message_t* msg, mavlink_object_detection_event_t* object_detection_event) +{ +#if MAVLINK_NEED_BYTE_SWAP + object_detection_event->time = mavlink_msg_object_detection_event_get_time(msg); + object_detection_event->bearing = mavlink_msg_object_detection_event_get_bearing(msg); + object_detection_event->distance = mavlink_msg_object_detection_event_get_distance(msg); + object_detection_event->object_id = mavlink_msg_object_detection_event_get_object_id(msg); + object_detection_event->type = mavlink_msg_object_detection_event_get_type(msg); + mavlink_msg_object_detection_event_get_name(msg, object_detection_event->name); + object_detection_event->quality = mavlink_msg_object_detection_event_get_quality(msg); +#else + memcpy(object_detection_event, msg->payload, 36); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h index 3dc4893fc62ee423196ce2562704d1a92fdf9d9f..c0db135d65b80dd7a4e92fe4541c98c0c50bf4ce 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h @@ -1,22 +1,35 @@ // MESSAGE OPTICAL_FLOW PACKING #define MAVLINK_MSG_ID_OPTICAL_FLOW 100 -#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 18 -#define MAVLINK_MSG_100_LEN 18 -#define MAVLINK_MSG_ID_OPTICAL_FLOW_KEY 0x4A -#define MAVLINK_MSG_100_KEY 0x4A -typedef struct __mavlink_optical_flow_t +typedef struct __mavlink_optical_flow_t { - uint64_t time; ///< Timestamp (UNIX) - float ground_distance; ///< Ground distance in meters - int16_t flow_x; ///< Flow in pixels in x-sensor direction - int16_t flow_y; ///< Flow in pixels in y-sensor direction - uint8_t sensor_id; ///< Sensor ID - uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality - + uint64_t time; ///< Timestamp (UNIX) + float ground_distance; ///< Ground distance in meters + int16_t flow_x; ///< Flow in pixels in x-sensor direction + int16_t flow_y; ///< Flow in pixels in y-sensor direction + uint8_t sensor_id; ///< Sensor ID + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality } mavlink_optical_flow_t; +#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 18 +#define MAVLINK_MSG_ID_100_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ + "OPTICAL_FLOW", \ + 6, \ + { { "time", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time) }, \ + { "ground_distance", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + { "flow_x", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_optical_flow_t, flow_x) }, \ + { "flow_y", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_optical_flow_t, flow_y) }, \ + { "sensor_id", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_optical_flow_t, sensor_id) }, \ + { "quality", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_optical_flow_t, quality) }, \ + } \ +} + + /** * @brief Pack a optical_flow message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_optical_flow_t * @param ground_distance Ground distance in meters * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - p->time = time; // uint64_t:Timestamp (UNIX) - p->sensor_id = sensor_id; // uint8_t:Sensor ID - p->flow_x = flow_x; // int16_t:Flow in pixels in x-sensor direction - p->flow_y = flow_y; // int16_t:Flow in pixels in y-sensor direction - p->quality = quality; // uint8_t:Optical flow quality / confidence. 0: bad, 255: maximum quality - p->ground_distance = ground_distance; // float:Ground distance in meters + put_uint64_t_by_index(msg, 0, time); // Timestamp (UNIX) + put_float_by_index(msg, 8, ground_distance); // Ground distance in meters + put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction + put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction + put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID + put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 146); } /** - * @brief Pack a optical_flow message + * @brief Pack a optical_flow message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t * @param ground_distance Ground distance in meters * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance) +{ + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; + + put_uint64_t_by_index(msg, 0, time); // Timestamp (UNIX) + put_float_by_index(msg, 8, ground_distance); // Ground distance in meters + put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction + put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction + put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID + put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 146); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a optical_flow message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + */ +static inline void mavlink_msg_optical_flow_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t time,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - p->time = time; // uint64_t:Timestamp (UNIX) - p->sensor_id = sensor_id; // uint8_t:Sensor ID - p->flow_x = flow_x; // int16_t:Flow in pixels in x-sensor direction - p->flow_y = flow_y; // int16_t:Flow in pixels in y-sensor direction - p->quality = quality; // uint8_t:Optical flow quality / confidence. 0: bad, 255: maximum quality - p->ground_distance = ground_distance; // float:Ground distance in meters + put_uint64_t_by_index(msg, 0, time); // Timestamp (UNIX) + put_float_by_index(msg, 8, ground_distance); // Ground distance in meters + put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction + put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction + put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID + put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 146); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a optical_flow struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->quality, optical_flow->ground_distance); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a optical_flow message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance Ground distance in meters */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) { - mavlink_header_t hdr; - mavlink_optical_flow_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN ) - payload.time = time; // uint64_t:Timestamp (UNIX) - payload.sensor_id = sensor_id; // uint8_t:Sensor ID - payload.flow_x = flow_x; // int16_t:Flow in pixels in x-sensor direction - payload.flow_y = flow_y; // int16_t:Flow in pixels in y-sensor direction - payload.quality = quality; // uint8_t:Optical flow quality / confidence. 0: bad, 255: maximum quality - payload.ground_distance = ground_distance; // float:Ground distance in meters - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_OPTICAL_FLOW_LEN; - hdr.msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x4A, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_optical_flow_pack_chan_send(chan, msg, time, sensor_id, flow_x, flow_y, quality, ground_distance); } #endif + // MESSAGE OPTICAL_FLOW UNPACKING + /** * @brief Get field time from optical_flow message * @@ -142,8 +164,7 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_optical_flow_get_time(const mavlink_message_t* msg) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; - return (uint64_t)(p->time); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -153,8 +174,7 @@ static inline uint64_t mavlink_msg_optical_flow_get_time(const mavlink_message_t */ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; - return (uint8_t)(p->sensor_id); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -164,8 +184,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_messa */ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; - return (int16_t)(p->flow_x); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -175,8 +194,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_ */ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; - return (int16_t)(p->flow_y); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -186,8 +204,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_ */ static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; - return (uint8_t)(p->quality); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -197,8 +214,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message */ static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg) { - mavlink_optical_flow_t *p = (mavlink_optical_flow_t *)&msg->payload[0]; - return (float)(p->ground_distance); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -209,5 +225,14 @@ static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_m */ static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow) { - memcpy( optical_flow, msg->payload, sizeof(mavlink_optical_flow_t)); +#if MAVLINK_NEED_BYTE_SWAP + optical_flow->time = mavlink_msg_optical_flow_get_time(msg); + optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg); + optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg); + optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); + optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); + optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); +#else + memcpy(optical_flow, MAVLINK_PAYLOAD(msg), 18); +#endif } 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 fa67beea00a13e24f9e5fb6ffc9cfb2cd42e8787..ddd66e7814e304428464b2ae6c3ff3464af48a98 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -1,18 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_KEY 0x22 -#define MAVLINK_MSG_21_KEY 0x22 -typedef struct __mavlink_param_request_list_t +typedef struct __mavlink_param_request_list_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_param_request_list_t; +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_21_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ + "PARAM_REQUEST_LIST", \ + 2, \ + { { "target_system", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ + } \ +} + + /** * @brief Pack a param_request_list message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_param_request_list_t * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 159); } /** - * @brief Pack a param_request_list message + * @brief Pack a param_request_list message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 159); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a param_request_list message on a channel and send + * @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 + */ +static inline void mavlink_msg_param_request_list_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) { - mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 159); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a param_request_list struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_header_t hdr; - mavlink_param_request_list_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x22, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_param_request_list_pack_chan_send(chan, msg, target_system, target_component); } #endif + // MESSAGE PARAM_REQUEST_LIST UNPACKING + /** * @brief Get field target_system from param_request_list message * @@ -114,8 +128,7 @@ 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) { - mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( param_request_list, msg->payload, sizeof(mavlink_param_request_list_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(param_request_list, MAVLINK_PAYLOAD(msg), 2); +#endif } 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 6cadd266ea52086a3b0aecbb78daddb621605fe6..c53fe9025a2a39debb50b71aee07e18752cca950 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -1,21 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_KEY 0x21 -#define MAVLINK_MSG_20_KEY 0x21 -typedef struct __mavlink_param_request_read_t +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 - char param_id[16]; ///< Onboard parameter id - + 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 + char param_id[16]; ///< Onboard parameter id } mavlink_param_request_read_t; + +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_ID_20_LEN 20 + #define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \ + "PARAM_REQUEST_READ", \ + 4, \ + { { "param_index", MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_request_read_t, target_component) }, \ + { "param_id", MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_request_read_t, param_id) }, \ + } \ +} + + /** * @brief Pack a param_request_read message * @param system_id ID of this system @@ -28,21 +38,21 @@ 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 char* 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) { - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - 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 + put_int16_t_by_index(msg, 0, param_index); // Parameter index. Send -1 to use the param ID field as identifier + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID + put_char_array_by_index(msg, 4, param_id, 16); // Onboard parameter id - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 214); } /** - * @brief Pack a param_request_read message + * @brief Pack a param_request_read message on a channel * @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 @@ -53,18 +63,46 @@ 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 char* 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) +{ + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + + put_int16_t_by_index(msg, 0, param_index); // Parameter index. Send -1 to use the param ID field as identifier + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID + put_char_array_by_index(msg, 4, param_id, 16); // Onboard parameter id + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 214); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a param_request_read message on a channel and send + * @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 param_id Onboard parameter id + * @param param_index Parameter index. Send -1 to use the param ID field as identifier + */ +static inline void mavlink_msg_param_request_read_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index) { - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - 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 + put_int16_t_by_index(msg, 0, param_index); // Parameter index. Send -1 to use the param ID field as identifier + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID + put_char_array_by_index(msg, 4, param_id, 16); // Onboard parameter id - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); + mavlink_finalize_message_chan_send(msg, chan, 20, 214); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a param_request_read struct into a message @@ -79,8 +117,6 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message @@ -90,38 +126,19 @@ 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 */ -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) +#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_header_t hdr; - mavlink_param_request_read_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - memcpy(payload.param_id, param_id, sizeof(payload.param_id)); // char[16]:Onboard parameter id - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x21, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_param_request_read_pack_chan_send(chan, msg, target_system, target_component, param_id, param_index); } #endif + // MESSAGE PARAM_REQUEST_READ UNPACKING + /** * @brief Get field target_system from param_request_read message * @@ -129,8 +146,7 @@ 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) { - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -140,8 +156,7 @@ 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) { - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -149,12 +164,9 @@ 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, char* param_id) +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(param_id, p->param_id, sizeof(p->param_id)); - return sizeof(p->param_id); + return MAVLINK_MSG_RETURN_char_array(msg, param_id, 16, 4); } /** @@ -164,8 +176,7 @@ 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) { - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; - return (int16_t)(p->param_index); + return MAVLINK_MSG_RETURN_int16_t(msg, 0); } /** @@ -176,5 +187,12 @@ 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) { - memcpy( param_request_read, msg->payload, sizeof(mavlink_param_request_read_t)); +#if MAVLINK_NEED_BYTE_SWAP + param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); + 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); +#else + memcpy(param_request_read, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index c786469549eac412fa31031b1d039cb57b523a69..5663829e04bc48a6d66a9486fdc0c1b9d9dca5ea 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -1,22 +1,33 @@ // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 -#define MAVLINK_MSG_ID_PARAM_SET_LEN 23 -#define MAVLINK_MSG_23_LEN 23 -#define MAVLINK_MSG_ID_PARAM_SET_KEY 0x37 -#define MAVLINK_MSG_23_KEY 0x37 -typedef struct __mavlink_param_set_t +typedef struct __mavlink_param_set_t { - float param_value; ///< Onboard parameter value - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - char param_id[16]; ///< Onboard parameter id - uint8_t param_type; ///< Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - + float param_value; ///< Onboard parameter value + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + char param_id[16]; ///< Onboard parameter id + uint8_t param_type; ///< Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t } mavlink_param_set_t; + +#define MAVLINK_MSG_ID_PARAM_SET_LEN 23 +#define MAVLINK_MSG_ID_23_LEN 23 + #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MESSAGE_INFO_PARAM_SET { \ + "PARAM_SET", \ + 5, \ + { { "param_value", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_set_t, target_component) }, \ + { "param_id", MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_set_t, param_id) }, \ + { "param_type", MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_set_t, param_type) }, \ + } \ +} + + /** * @brief Pack a param_set message * @param system_id ID of this system @@ -30,22 +41,22 @@ typedef struct __mavlink_param_set_t * @param param_type Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t * @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 char* param_id, float param_value, uint8_t param_type) +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, uint8_t param_type) { - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - 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 - p->param_type = param_type; // uint8_t:Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + put_float_by_index(msg, 0, param_value); // Onboard parameter value + put_uint8_t_by_index(msg, 4, target_system); // System ID + put_uint8_t_by_index(msg, 5, target_component); // Component ID + put_char_array_by_index(msg, 6, param_id, 16); // Onboard parameter id + put_uint8_t_by_index(msg, 22, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 23, 168); } /** - * @brief Pack a param_set message + * @brief Pack a param_set message on a channel * @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 @@ -57,20 +68,50 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @param param_type Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t * @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 char* param_id, float param_value, uint8_t param_type) +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,uint8_t param_type) { - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - 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 - p->param_type = param_type; // uint8_t:Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + put_float_by_index(msg, 0, param_value); // Onboard parameter value + put_uint8_t_by_index(msg, 4, target_system); // System ID + put_uint8_t_by_index(msg, 5, target_component); // Component ID + put_char_array_by_index(msg, 6, param_id, 16); // Onboard parameter id + put_uint8_t_by_index(msg, 22, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23, 168); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a param_set message on a channel and send + * @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 param_id Onboard parameter id + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + */ +static inline void mavlink_msg_param_set_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,float param_value,uint8_t param_type) +{ + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; + + put_float_by_index(msg, 0, param_value); // Onboard parameter value + put_uint8_t_by_index(msg, 4, target_system); // System ID + put_uint8_t_by_index(msg, 5, target_component); // Component ID + put_char_array_by_index(msg, 6, param_id, 16); // Onboard parameter id + put_uint8_t_by_index(msg, 22, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + + mavlink_finalize_message_chan_send(msg, chan, 23, 168); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a param_set struct into a message * @@ -84,8 +125,6 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_set message * @param chan MAVLink channel to send the message @@ -96,39 +135,19 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_value Onboard parameter value * @param param_type Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t */ -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, uint8_t param_type) +#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, uint8_t param_type) { - mavlink_header_t hdr; - mavlink_param_set_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_SET_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - memcpy(payload.param_id, param_id, sizeof(payload.param_id)); // char[16]:Onboard parameter id - payload.param_value = param_value; // float:Onboard parameter value - payload.param_type = param_type; // uint8_t:Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x37, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 23); + mavlink_msg_param_set_pack_chan_send(chan, msg, target_system, target_component, param_id, param_value, param_type); } #endif + // MESSAGE PARAM_SET UNPACKING + /** * @brief Get field target_system from param_set message * @@ -136,8 +155,7 @@ 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) { - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -147,8 +165,7 @@ 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) { - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -156,12 +173,9 @@ 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, char* param_id) +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(param_id, p->param_id, sizeof(p->param_id)); - return sizeof(p->param_id); + return MAVLINK_MSG_RETURN_char_array(msg, param_id, 16, 6); } /** @@ -171,8 +185,7 @@ 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) { - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - return (float)(p->param_value); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -182,8 +195,7 @@ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_ */ static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message_t* msg) { - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - return (uint8_t)(p->param_type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 22); } /** @@ -194,5 +206,13 @@ static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message */ static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) { - memcpy( param_set, msg->payload, sizeof(mavlink_param_set_t)); +#if MAVLINK_NEED_BYTE_SWAP + param_set->param_value = mavlink_msg_param_set_get_param_value(msg); + 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_type = mavlink_msg_param_set_get_param_type(msg); +#else + memcpy(param_set, MAVLINK_PAYLOAD(msg), 23); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 70f3a20490023b297631097e0732d9657c0b5bea..dc7092add0f1f79d25926b700a0be7e9ec6f92aa 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -1,22 +1,33 @@ // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 -#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 25 -#define MAVLINK_MSG_22_LEN 25 -#define MAVLINK_MSG_ID_PARAM_VALUE_KEY 0xBF -#define MAVLINK_MSG_22_KEY 0xBF -typedef struct __mavlink_param_value_t +typedef struct __mavlink_param_value_t { - 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 - uint8_t param_type; ///< Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - + 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 + uint8_t param_type; ///< Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t } mavlink_param_value_t; + +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 25 +#define MAVLINK_MSG_ID_22_LEN 25 + #define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \ + "PARAM_VALUE", \ + 5, \ + { { "param_value", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \ + { "param_count", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_param_value_t, param_count) }, \ + { "param_index", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_param_value_t, param_index) }, \ + { "param_id", MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \ + { "param_type", MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_param_value_t, param_type) }, \ + } \ +} + + /** * @brief Pack a param_value message * @param system_id ID of this system @@ -30,22 +41,22 @@ 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 char* param_id, float param_value, uint8_t param_type, 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, uint8_t param_type, uint16_t param_count, uint16_t param_index) { - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - 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_type = param_type; // uint8_t:Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + put_float_by_index(msg, 0, param_value); // Onboard parameter value + put_uint16_t_by_index(msg, 4, param_count); // Total number of onboard parameters + put_uint16_t_by_index(msg, 6, param_index); // Index of this onboard parameter + put_char_array_by_index(msg, 8, param_id, 16); // Onboard parameter id + put_uint8_t_by_index(msg, 24, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 25, 220); } /** - * @brief Pack a param_value message + * @brief Pack a param_value message on a channel * @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 @@ -57,20 +68,50 @@ 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 char* param_id, float param_value, uint8_t param_type, 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,uint8_t param_type,uint16_t param_count,uint16_t param_index) { - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - 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_type = param_type; // uint8_t:Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + put_float_by_index(msg, 0, param_value); // Onboard parameter value + put_uint16_t_by_index(msg, 4, param_count); // Total number of onboard parameters + put_uint16_t_by_index(msg, 6, param_index); // Index of this onboard parameter + put_char_array_by_index(msg, 8, param_id, 16); // Onboard parameter id + put_uint8_t_by_index(msg, 24, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 220); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a param_value message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param param_id Onboard parameter id + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + */ +static inline void mavlink_msg_param_value_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + const char *param_id,float param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) +{ + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; + + put_float_by_index(msg, 0, param_value); // Onboard parameter value + put_uint16_t_by_index(msg, 4, param_count); // Total number of onboard parameters + put_uint16_t_by_index(msg, 6, param_index); // Index of this onboard parameter + put_char_array_by_index(msg, 8, param_id, 16); // Onboard parameter id + put_uint8_t_by_index(msg, 24, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + + mavlink_finalize_message_chan_send(msg, chan, 25, 220); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a param_value struct into a message * @@ -84,8 +125,6 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_value message * @param chan MAVLink channel to send the message @@ -96,50 +135,27 @@ 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 */ -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint8_t param_type, 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, uint8_t param_type, uint16_t param_count, uint16_t param_index) { - mavlink_header_t hdr; - mavlink_param_value_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN ) - memcpy(payload.param_id, param_id, sizeof(payload.param_id)); // char[16]:Onboard parameter id - payload.param_value = param_value; // float:Onboard parameter value - payload.param_type = param_type; // uint8_t:Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - payload.param_count = param_count; // uint16_t:Total number of onboard parameters - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xBF, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 25); + mavlink_msg_param_value_pack_chan_send(chan, msg, param_id, param_value, param_type, param_count, param_index); } #endif + // MESSAGE PARAM_VALUE UNPACKING + /** * @brief Get field param_id from param_value message * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char* param_id) +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(param_id, p->param_id, sizeof(p->param_id)); - return sizeof(p->param_id); + return MAVLINK_MSG_RETURN_char_array(msg, param_id, 16, 8); } /** @@ -149,8 +165,7 @@ 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) { - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - return (float)(p->param_value); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -160,8 +175,7 @@ static inline float mavlink_msg_param_value_get_param_value(const mavlink_messag */ static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_message_t* msg) { - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - return (uint8_t)(p->param_type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 24); } /** @@ -171,8 +185,7 @@ static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_messa */ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) { - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - return (uint16_t)(p->param_count); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -182,8 +195,7 @@ 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) { - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - return (uint16_t)(p->param_index); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -194,5 +206,13 @@ 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) { - memcpy( param_value, msg->payload, sizeof(mavlink_param_value_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + mavlink_msg_param_value_get_param_id(msg, param_value->param_id); + param_value->param_type = mavlink_msg_param_value_get_param_type(msg); +#else + memcpy(param_value, MAVLINK_PAYLOAD(msg), 25); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index faee1273211f1100dc4dbe22cc89ae3d6bb4dc6d..3a58f71a84c674d652022e36b9845e183156faf9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -1,20 +1,31 @@ // MESSAGE PING PACKING #define MAVLINK_MSG_ID_PING 4 -#define MAVLINK_MSG_ID_PING_LEN 14 -#define MAVLINK_MSG_4_LEN 14 -#define MAVLINK_MSG_ID_PING_KEY 0xE2 -#define MAVLINK_MSG_4_KEY 0xE2 -typedef struct __mavlink_ping_t +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 + 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 } mavlink_ping_t; +#define MAVLINK_MSG_ID_PING_LEN 14 +#define MAVLINK_MSG_ID_4_LEN 14 + + + +#define MAVLINK_MESSAGE_INFO_PING { \ + "PING", \ + 4, \ + { { "time", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_ping_t, time) }, \ + { "seq", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ping_t, seq) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_ping_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_ping_t, target_component) }, \ + } \ +} + + /** * @brief Pack a ping message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_ping_t * @param time Unix timestamp in microseconds * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - 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 + put_uint64_t_by_index(msg, 0, time); // Unix timestamp in microseconds + put_uint32_t_by_index(msg, 8, seq); // PING sequence + put_uint8_t_by_index(msg, 12, 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 + put_uint8_t_by_index(msg, 13, 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 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 14, 105); } /** - * @brief Pack a ping message + * @brief Pack a ping message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen * @param time Unix timestamp in microseconds * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_PING; + + put_uint64_t_by_index(msg, 0, time); // Unix timestamp in microseconds + put_uint32_t_by_index(msg, 8, seq); // PING sequence + put_uint8_t_by_index(msg, 12, 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 + put_uint8_t_by_index(msg, 13, 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 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 105); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a ping message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param seq PING sequence + * @param 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 + * @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 + */ +static inline void mavlink_msg_ping_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint32_t seq,uint8_t target_system,uint8_t target_component,uint64_t time) { - mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - 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 + put_uint64_t_by_index(msg, 0, time); // Unix timestamp in microseconds + put_uint32_t_by_index(msg, 8, seq); // PING sequence + put_uint8_t_by_index(msg, 12, 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 + put_uint8_t_by_index(msg, 13, 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 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); + mavlink_finalize_message_chan_send(msg, chan, 14, 105); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a ping struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon return mavlink_msg_ping_pack(system_id, component_id, msg, ping->seq, ping->target_system, ping->target_component, ping->time); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a ping message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + 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_header_t hdr; - mavlink_ping_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PING_LEN ) - payload.seq = seq; // uint32_t:PING sequence - payload.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 - payload.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 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xE2, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 14); + mavlink_msg_ping_pack_chan_send(chan, msg, seq, target_system, target_component, time); } #endif + // MESSAGE PING UNPACKING + /** * @brief Get field seq from ping message * @@ -128,8 +146,7 @@ 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) { - mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; - return (uint32_t)(p->seq); + return MAVLINK_MSG_RETURN_uint32_t(msg, 8); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 12); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 13); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; - return (uint64_t)(p->time); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( ping, msg->payload, sizeof(mavlink_ping_t)); +#if MAVLINK_NEED_BYTE_SWAP + ping->time = mavlink_msg_ping_get_time(msg); + 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); +#else + memcpy(ping, MAVLINK_PAYLOAD(msg), 14); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index f1f8def94b8b3e1e854529aaa4fefaf6c31d5fce..dd4af078c3b1a10e2701637dcaa6dd25283eac49 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -1,20 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_POSITION_TARGET_KEY 0x4B -#define MAVLINK_MSG_63_KEY 0x4B -typedef struct __mavlink_position_target_t +typedef struct __mavlink_position_target_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< yaw orientation in radians, 0 = NORTH } mavlink_position_target_t; +#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 +#define MAVLINK_MSG_ID_63_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_TARGET { \ + "POSITION_TARGET", \ + 4, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_target_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_t, yaw) }, \ + } \ +} + + /** * @brief Pack a position_target message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_position_target_t * @param yaw yaw orientation in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 16, 126); } /** - * @brief Pack a position_target message + * @brief Pack a position_target message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 * @param yaw yaw orientation in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; + + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 126); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a position_target message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param x x position + * @param y y position + * @param z z position + * @param yaw yaw orientation in radians, 0 = NORTH + */ +static inline void mavlink_msg_position_target_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float x,float y,float z,float yaw) { - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN); + mavlink_finalize_message_chan_send(msg, chan, 16, 126); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a position_target struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin return mavlink_msg_position_target_pack(system_id, component_id, msg, position_target->x, position_target->y, position_target->z, position_target->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_target message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - mavlink_header_t hdr; - mavlink_position_target_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN ) - payload.x = x; // float:x position - payload.y = y; // float:y position - payload.z = z; // float:z position - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x4B, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 16); + mavlink_msg_position_target_pack_chan_send(chan, msg, x, y, z, yaw); } #endif + // MESSAGE POSITION_TARGET UNPACKING + /** * @brief Get field x from position_target message * @@ -128,8 +146,7 @@ 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) { - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( position_target, msg->payload, sizeof(mavlink_position_target_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(position_target, MAVLINK_PAYLOAD(msg), 16); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index b7b72c92fb608ed9e8bc117a851798fb4ee302b3..dea5468b91ee2cad86507999a9e3c32f13303525 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -1,26 +1,43 @@ // 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 -#define MAVLINK_MSG_ID_RAW_IMU_KEY 0x1C -#define MAVLINK_MSG_28_KEY 0x1C -typedef struct __mavlink_raw_imu_t +typedef struct __mavlink_raw_imu_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t xacc; ///< X acceleration (raw) - int16_t yacc; ///< Y acceleration (raw) - int16_t zacc; ///< Z acceleration (raw) - int16_t xgyro; ///< Angular speed around X axis (raw) - int16_t ygyro; ///< Angular speed around Y axis (raw) - int16_t zgyro; ///< Angular speed around Z axis (raw) - int16_t xmag; ///< X Magnetic field (raw) - int16_t ymag; ///< Y Magnetic field (raw) - int16_t zmag; ///< Z Magnetic field (raw) - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (raw) + int16_t yacc; ///< Y acceleration (raw) + int16_t zacc; ///< Z acceleration (raw) + int16_t xgyro; ///< Angular speed around X axis (raw) + int16_t ygyro; ///< Angular speed around Y axis (raw) + int16_t zgyro; ///< Angular speed around Z axis (raw) + int16_t xmag; ///< X Magnetic field (raw) + int16_t ymag; ///< Y Magnetic field (raw) + int16_t zmag; ///< Z Magnetic field (raw) } mavlink_raw_imu_t; +#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_ID_28_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_RAW_IMU { \ + "RAW_IMU", \ + 10, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, usec) }, \ + { "xacc", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ + { "yacc", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ + { "zacc", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_imu_t, zacc) }, \ + { "xgyro", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_imu_t, xgyro) }, \ + { "ygyro", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_raw_imu_t, ygyro) }, \ + { "zgyro", MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_raw_imu_t, zgyro) }, \ + { "xmag", MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ + { "ymag", MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ + { "zmag", MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + } \ +} + + /** * @brief Pack a raw_imu message * @param system_id ID of this system @@ -39,27 +56,27 @@ typedef struct __mavlink_raw_imu_t * @param zmag Z Magnetic field (raw) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - 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, MAVLINK_MSG_ID_RAW_IMU_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (raw) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (raw) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (raw) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (raw) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (raw) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (raw) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (raw) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (raw) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (raw) + + return mavlink_finalize_message(msg, system_id, component_id, 26, 179); } /** - * @brief Pack a raw_imu message + * @brief Pack a raw_imu message on a channel * @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 @@ -76,24 +93,64 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @param zmag Z Magnetic field (raw) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (raw) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (raw) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (raw) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (raw) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (raw) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (raw) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (raw) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (raw) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (raw) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 179); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a raw_imu message on a channel and send + * @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 xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + */ +static inline void mavlink_msg_raw_imu_pack_chan_send(mavlink_channel_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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - 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, MAVLINK_MSG_ID_RAW_IMU_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (raw) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (raw) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (raw) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (raw) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (raw) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (raw) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (raw) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (raw) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (raw) + + mavlink_finalize_message_chan_send(msg, chan, 26, 179); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a raw_imu struct into a message @@ -108,8 +165,6 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a raw_imu message * @param chan MAVLink channel to send the message @@ -125,44 +180,19 @@ 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 + 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_header_t hdr; - mavlink_raw_imu_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RAW_IMU_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.xacc = xacc; // int16_t:X acceleration (raw) - payload.yacc = yacc; // int16_t:Y acceleration (raw) - payload.zacc = zacc; // int16_t:Z acceleration (raw) - payload.xgyro = xgyro; // int16_t:Angular speed around X axis (raw) - payload.ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) - payload.zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) - payload.xmag = xmag; // int16_t:X Magnetic field (raw) - payload.ymag = ymag; // int16_t:Y Magnetic field (raw) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x1C, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 26); + mavlink_msg_raw_imu_pack_chan_send(chan, msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); } #endif + // MESSAGE RAW_IMU UNPACKING + /** * @brief Get field usec from raw_imu message * @@ -170,8 +200,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -181,8 +210,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->xacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 8); } /** @@ -192,8 +220,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->yacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 10); } /** @@ -203,8 +230,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->zacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -214,8 +240,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->xgyro); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -225,8 +250,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->ygyro); + return MAVLINK_MSG_RETURN_int16_t(msg, 16); } /** @@ -236,8 +260,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->zgyro); + return MAVLINK_MSG_RETURN_int16_t(msg, 18); } /** @@ -247,8 +270,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->xmag); + return MAVLINK_MSG_RETURN_int16_t(msg, 20); } /** @@ -258,8 +280,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->ymag); + return MAVLINK_MSG_RETURN_int16_t(msg, 22); } /** @@ -269,8 +290,7 @@ 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) { - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; - return (int16_t)(p->zmag); + return MAVLINK_MSG_RETURN_int16_t(msg, 24); } /** @@ -281,5 +301,18 @@ 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) { - memcpy( raw_imu, msg->payload, sizeof(mavlink_raw_imu_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(raw_imu, MAVLINK_PAYLOAD(msg), 26); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 20df43a187922d1dd6e809fe4272a46fbb5b2b1d..24d0e054724908e5cce16226b30252e1ef9de730 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -1,21 +1,33 @@ // 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 -#define MAVLINK_MSG_ID_RAW_PRESSURE_KEY 0x15 -#define MAVLINK_MSG_29_KEY 0x15 -typedef struct __mavlink_raw_pressure_t +typedef struct __mavlink_raw_pressure_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t press_abs; ///< Absolute pressure (raw) - int16_t press_diff1; ///< Differential pressure 1 (raw) - int16_t press_diff2; ///< Differential pressure 2 (raw) - int16_t temperature; ///< Raw Temperature measurement (raw) - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t press_abs; ///< Absolute pressure (raw) + int16_t press_diff1; ///< Differential pressure 1 (raw) + int16_t press_diff2; ///< Differential pressure 2 (raw) + int16_t temperature; ///< Raw Temperature measurement (raw) } mavlink_raw_pressure_t; +#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 +#define MAVLINK_MSG_ID_29_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_RAW_PRESSURE { \ + "RAW_PRESSURE", \ + 5, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_pressure_t, usec) }, \ + { "press_abs", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_pressure_t, press_abs) }, \ + { "press_diff1", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_pressure_t, press_diff1) }, \ + { "press_diff2", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_pressure_t, press_diff2) }, \ + { "temperature", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_pressure_t, temperature) }, \ + } \ +} + + /** * @brief Pack a raw_pressure message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_raw_pressure_t * @param temperature Raw Temperature measurement (raw) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, press_abs); // Absolute pressure (raw) + put_int16_t_by_index(msg, 10, press_diff1); // Differential pressure 1 (raw) + put_int16_t_by_index(msg, 12, press_diff2); // Differential pressure 2 (raw) + put_int16_t_by_index(msg, 14, temperature); // Raw Temperature measurement (raw) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 16, 136); } /** - * @brief Pack a raw_pressure message + * @brief Pack a raw_pressure message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t * @param temperature Raw Temperature measurement (raw) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, press_abs); // Absolute pressure (raw) + put_int16_t_by_index(msg, 10, press_diff1); // Differential pressure 1 (raw) + put_int16_t_by_index(msg, 12, press_diff2); // Differential pressure 2 (raw) + put_int16_t_by_index(msg, 14, temperature); // Raw Temperature measurement (raw) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 136); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a raw_pressure message on a channel and send + * @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 press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + */ +static inline void mavlink_msg_raw_pressure_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,int16_t press_abs,int16_t press_diff1,int16_t press_diff2,int16_t temperature) +{ + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, press_abs); // Absolute pressure (raw) + put_int16_t_by_index(msg, 10, press_diff1); // Differential pressure 1 (raw) + put_int16_t_by_index(msg, 12, press_diff2); // Differential pressure 2 (raw) + put_int16_t_by_index(msg, 14, temperature); // Raw Temperature measurement (raw) + + mavlink_finalize_message_chan_send(msg, chan, 16, 136); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a raw_pressure struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a raw_pressure message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ 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 + 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_header_t hdr; - mavlink_raw_pressure_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.press_abs = press_abs; // int16_t:Absolute pressure (raw) - payload.press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) - payload.press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x15, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 16); + mavlink_msg_raw_pressure_pack_chan_send(chan, msg, usec, press_abs, press_diff1, press_diff2, temperature); } #endif + // MESSAGE RAW_PRESSURE UNPACKING + /** * @brief Get field usec from raw_pressure message * @@ -135,8 +155,7 @@ 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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -146,8 +165,7 @@ 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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; - return (int16_t)(p->press_abs); + return MAVLINK_MSG_RETURN_int16_t(msg, 8); } /** @@ -157,8 +175,7 @@ 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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; - return (int16_t)(p->press_diff1); + return MAVLINK_MSG_RETURN_int16_t(msg, 10); } /** @@ -168,8 +185,7 @@ 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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; - return (int16_t)(p->press_diff2); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -179,8 +195,7 @@ 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) { - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; - return (int16_t)(p->temperature); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -191,5 +206,13 @@ 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) { - memcpy( raw_pressure, msg->payload, sizeof(mavlink_raw_pressure_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(raw_pressure, MAVLINK_PAYLOAD(msg), 16); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h index 15a3b138b8e660c37f4dc54de4b4dcae1a9fc6c6..f0119624804b22178fe3b02baf180d7d724c8c38 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -1,26 +1,43 @@ // 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 -#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_KEY 0xC8 -#define MAVLINK_MSG_70_KEY 0xC8 -typedef struct __mavlink_rc_channels_override_t +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 - + 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; +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_ID_70_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ + "RC_CHANNELS_OVERRIDE", \ + 10, \ + { { "chan1_raw", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ + { "chan2_raw", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_override_t, chan2_raw) }, \ + { "chan3_raw", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_override_t, chan3_raw) }, \ + { "chan4_raw", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_override_t, chan4_raw) }, \ + { "chan5_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_override_t, chan5_raw) }, \ + { "chan6_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ + { "chan7_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ + { "chan8_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ + } \ +} + + /** * @brief Pack a rc_channels_override message * @param system_id ID of this system @@ -39,27 +56,27 @@ typedef struct __mavlink_rc_channels_override_t * @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) +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); + put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + return mavlink_finalize_message(msg, system_id, component_id, 18, 124); } /** - * @brief Pack a rc_channels_override message + * @brief Pack a rc_channels_override message on a channel * @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 @@ -76,24 +93,64 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 124); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a rc_channels_override message on a channel and send + * @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 + */ +static inline void mavlink_msg_rc_channels_override_pack_chan_send(mavlink_channel_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); + put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 18, 124); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a rc_channels_override struct into a message @@ -108,8 +165,6 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id 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); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a rc_channels_override message * @param chan MAVLink channel to send the message @@ -125,44 +180,19 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id * @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; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - payload.chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - payload.chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - payload.chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - payload.chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - payload.chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - payload.chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xC8, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_rc_channels_override_pack_chan_send(chan, msg, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw); } #endif + // MESSAGE RC_CHANNELS_OVERRIDE UNPACKING + /** * @brief Get field target_system from rc_channels_override message * @@ -170,8 +200,7 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, */ 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); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -181,8 +210,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const m */ 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); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -192,8 +220,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -203,8 +230,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 2); } /** @@ -214,8 +240,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -225,8 +250,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -236,8 +260,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -247,8 +270,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 10); } /** @@ -258,8 +280,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -269,8 +290,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl */ 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); + return MAVLINK_MSG_RETURN_uint16_t(msg, 14); } /** @@ -281,5 +301,18 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavl */ 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)); +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg); + rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg); + rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg); + rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg); + rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg); + rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg); + rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg); + rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); + rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); + rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); +#else + memcpy(rc_channels_override, MAVLINK_PAYLOAD(msg), 18); +#endif } 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 f1fcb6cf4667e68a0174aa169af57f26bd9512f1..8e26fbcfcb06d66225c6634f39852eeec92b48ab 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -1,25 +1,41 @@ // 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 -#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_KEY 0x2B -#define MAVLINK_MSG_35_KEY 0x2B -typedef struct __mavlink_rc_channels_raw_t +typedef struct __mavlink_rc_channels_raw_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 rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - + 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 rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% } mavlink_rc_channels_raw_t; +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 17 +#define MAVLINK_MSG_ID_35_LEN 17 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW { \ + "RC_CHANNELS_RAW", \ + 9, \ + { { "chan1_raw", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_raw_t, chan1_raw) }, \ + { "chan2_raw", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_raw_t, chan2_raw) }, \ + { "chan3_raw", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_raw_t, chan3_raw) }, \ + { "chan4_raw", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_raw_t, chan4_raw) }, \ + { "chan5_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_raw_t, chan5_raw) }, \ + { "chan6_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_raw_t, chan6_raw) }, \ + { "chan7_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_raw_t, chan7_raw) }, \ + { "chan8_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_raw_t, chan8_raw) }, \ + { "rssi", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_raw_t, rssi) }, \ + } \ +} + + /** * @brief Pack a rc_channels_raw message * @param system_id ID of this system @@ -37,26 +53,26 @@ typedef struct __mavlink_rc_channels_raw_t * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - 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, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); + put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + return mavlink_finalize_message(msg, system_id, component_id, 17, 252); } /** - * @brief Pack a rc_channels_raw message + * @brief Pack a rc_channels_raw message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - 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, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); + put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17, 252); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a rc_channels_raw message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @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 + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + */ +static inline void mavlink_msg_rc_channels_raw_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + + put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds + put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds + put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds + put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds + put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds + put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds + put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds + put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + mavlink_finalize_message_chan_send(msg, chan, 17, 252); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a rc_channels_raw struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a rc_channels_raw message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_rc_channels_raw_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN ) - payload.chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - payload.chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - payload.chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - payload.chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - payload.chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - payload.chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - payload.chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - payload.chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x2B, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 17); + mavlink_msg_rc_channels_raw_pack_chan_send(chan, msg, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi); } #endif + // MESSAGE RC_CHANNELS_RAW UNPACKING + /** * @brief Get field chan1_raw from rc_channels_raw message * @@ -163,8 +191,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan1_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan2_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 2); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan3_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan4_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan5_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan6_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 10); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan7_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint16_t)(p->chan8_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 14); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; - return (uint8_t)(p->rssi); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( rc_channels_raw, msg->payload, sizeof(mavlink_rc_channels_raw_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(rc_channels_raw, MAVLINK_PAYLOAD(msg), 17); +#endif } 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 266f9bfe84cce87067870d871b21f336a9ddccfe..44845ded2d4606a9b0af7162f7f95e4a43503fb2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -1,25 +1,41 @@ // 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 -#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_KEY 0xC0 -#define MAVLINK_MSG_36_KEY 0xC0 -typedef struct __mavlink_rc_channels_scaled_t +typedef struct __mavlink_rc_channels_scaled_t { - int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% } mavlink_rc_channels_scaled_t; +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 17 +#define MAVLINK_MSG_ID_36_LEN 17 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED { \ + "RC_CHANNELS_SCALED", \ + 9, \ + { { "chan1_scaled", MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_rc_channels_scaled_t, chan1_scaled) }, \ + { "chan2_scaled", MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_rc_channels_scaled_t, chan2_scaled) }, \ + { "chan3_scaled", MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_rc_channels_scaled_t, chan3_scaled) }, \ + { "chan4_scaled", MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_rc_channels_scaled_t, chan4_scaled) }, \ + { "chan5_scaled", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rc_channels_scaled_t, chan5_scaled) }, \ + { "chan6_scaled", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rc_channels_scaled_t, chan6_scaled) }, \ + { "chan7_scaled", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_rc_channels_scaled_t, chan7_scaled) }, \ + { "chan8_scaled", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_rc_channels_scaled_t, chan8_scaled) }, \ + { "rssi", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_scaled_t, rssi) }, \ + } \ +} + + /** * @brief Pack a rc_channels_scaled message * @param system_id ID of this system @@ -37,26 +53,26 @@ typedef struct __mavlink_rc_channels_scaled_t * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - 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, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); + put_int16_t_by_index(msg, 0, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 2, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 4, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 6, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 8, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 10, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 12, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 14, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + return mavlink_finalize_message(msg, system_id, component_id, 17, 162); } /** - * @brief Pack a rc_channels_scaled message + * @brief Pack a rc_channels_scaled message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - 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, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); + put_int16_t_by_index(msg, 0, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 2, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 4, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 6, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 8, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 10, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 12, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 14, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17, 162); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a rc_channels_scaled message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + */ +static inline void mavlink_msg_rc_channels_scaled_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + + put_int16_t_by_index(msg, 0, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 2, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 4, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 6, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 8, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 10, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 12, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 14, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + mavlink_finalize_message_chan_send(msg, chan, 17, 162); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a rc_channels_scaled struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a rc_channels_scaled message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_rc_channels_scaled_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN ) - payload.chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xC0, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 17); + mavlink_msg_rc_channels_scaled_pack_chan_send(chan, msg, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi); } #endif + // MESSAGE RC_CHANNELS_SCALED UNPACKING + /** * @brief Get field chan1_scaled from rc_channels_scaled message * @@ -163,8 +191,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan1_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 0); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan2_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 2); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan3_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 4); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan4_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 6); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan5_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 8); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan6_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 10); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan7_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (int16_t)(p->chan8_scaled); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; - return (uint8_t)(p->rssi); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( rc_channels_scaled, msg->payload, sizeof(mavlink_rc_channels_scaled_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(rc_channels_scaled, MAVLINK_PAYLOAD(msg), 17); +#endif } 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 4b1986738bffd2f6885bc95ab1b1c13685dd48fc..44d51935dc0d98427600730733125dfd1776c1f3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -1,21 +1,33 @@ // 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 -#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_KEY 0x2A -#define MAVLINK_MSG_66_KEY 0x2A -typedef struct __mavlink_request_data_stream_t +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 data stream - uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. - + 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 data stream + uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 +#define MAVLINK_MSG_ID_66_LEN 6 + + + +#define MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM { \ + "REQUEST_DATA_STREAM", \ + 5, \ + { { "req_message_rate", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_request_data_stream_t, req_message_rate) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_request_data_stream_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_request_data_stream_t, target_component) }, \ + { "req_stream_id", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_request_data_stream_t, req_stream_id) }, \ + { "start_stop", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_data_stream_t, start_stop) }, \ + } \ +} + + /** * @brief Pack a request_data_stream message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_request_data_stream_t * @param start_stop 1 to start sending, 0 to stop sending. * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - 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 data stream - 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. + put_uint16_t_by_index(msg, 0, req_message_rate); // The requested interval between two messages of this type + put_uint8_t_by_index(msg, 2, target_system); // The target requested to send the message stream. + put_uint8_t_by_index(msg, 3, target_component); // The target requested to send the message stream. + put_uint8_t_by_index(msg, 4, req_stream_id); // The ID of the requested data stream + put_uint8_t_by_index(msg, 5, start_stop); // 1 to start sending, 0 to stop sending. - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 6, 148); } /** - * @brief Pack a request_data_stream message + * @brief Pack a request_data_stream message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @param start_stop 1 to start sending, 0 to stop sending. * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - 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 data stream - 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. + put_uint16_t_by_index(msg, 0, req_message_rate); // The requested interval between two messages of this type + put_uint8_t_by_index(msg, 2, target_system); // The target requested to send the message stream. + put_uint8_t_by_index(msg, 3, target_component); // The target requested to send the message stream. + put_uint8_t_by_index(msg, 4, req_stream_id); // The ID of the requested data stream + put_uint8_t_by_index(msg, 5, start_stop); // 1 to start sending, 0 to stop sending. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 148); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a request_data_stream message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested data stream + * @param req_message_rate The requested interval between two messages of this type + * @param start_stop 1 to start sending, 0 to stop sending. + */ +static inline void mavlink_msg_request_data_stream_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + + put_uint16_t_by_index(msg, 0, req_message_rate); // The requested interval between two messages of this type + put_uint8_t_by_index(msg, 2, target_system); // The target requested to send the message stream. + put_uint8_t_by_index(msg, 3, target_component); // The target requested to send the message stream. + put_uint8_t_by_index(msg, 4, req_stream_id); // The ID of the requested data stream + put_uint8_t_by_index(msg, 5, start_stop); // 1 to start sending, 0 to stop sending. + + mavlink_finalize_message_chan_send(msg, chan, 6, 148); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a request_data_stream struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a request_data_stream message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ 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 + 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_header_t hdr; - mavlink_request_data_stream_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN ) - payload.target_system = target_system; // uint8_t:The target requested to send the message stream. - payload.target_component = target_component; // uint8_t:The target requested to send the message stream. - payload.req_stream_id = req_stream_id; // uint8_t:The ID of the requested data stream - payload.req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x2A, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 6); + mavlink_msg_request_data_stream_pack_chan_send(chan, msg, target_system, target_component, req_stream_id, req_message_rate, start_stop); } #endif + // MESSAGE REQUEST_DATA_STREAM UNPACKING + /** * @brief Get field target_system from request_data_stream message * @@ -135,8 +155,7 @@ 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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -146,8 +165,7 @@ 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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -157,8 +175,7 @@ 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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; - return (uint8_t)(p->req_stream_id); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -168,8 +185,7 @@ 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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; - return (uint16_t)(p->req_message_rate); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -179,8 +195,7 @@ 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) { - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; - return (uint8_t)(p->start_stop); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -191,5 +206,13 @@ 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) { - memcpy( request_data_stream, msg->payload, sizeof(mavlink_request_data_stream_t)); +#if MAVLINK_NEED_BYTE_SWAP + request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); + 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->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); +#else + memcpy(request_data_stream, MAVLINK_PAYLOAD(msg), 6); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index 16fa0d0183f979eb15aeeb2cec74174c5b188006..e245845f67c8b3a54da796d437c1b9b3eb6cbb5b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -1,21 +1,33 @@ // MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING #define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 58 -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_58_LEN 20 -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_KEY 0xF5 -#define MAVLINK_MSG_58_KEY 0xF5 -typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t +typedef struct __mavlink_roll_pitch_yaw_speed_thrust_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 - float thrust; ///< Collective thrust, normalized to 0 .. 1 - + 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 + float thrust; ///< Collective thrust, normalized to 0 .. 1 } mavlink_roll_pitch_yaw_speed_thrust_setpoint_t; +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 20 +#define MAVLINK_MSG_ID_58_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT { \ + "ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT", \ + 5, \ + { { "time_ms", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, time_ms) }, \ + { "roll_speed", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, roll_speed) }, \ + { "pitch_speed", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, pitch_speed) }, \ + { "yaw_speed", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, yaw_speed) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, thrust) }, \ + } \ +} + + /** * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_uint32_t_by_index(msg, 0, time_ms); // Timestamp in milliseconds since system boot + put_float_by_index(msg, 4, roll_speed); // Desired roll angular speed in rad/s + put_float_by_index(msg, 8, pitch_speed); // Desired pitch angular speed in rad/s + put_float_by_index(msg, 12, yaw_speed); // Desired yaw angular speed in rad/s + put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 148); } /** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uin * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_ms,float roll_speed,float pitch_speed,float yaw_speed,float thrust) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_uint32_t_by_index(msg, 0, time_ms); // Timestamp in milliseconds since system boot + put_float_by_index(msg, 4, roll_speed); // Desired roll angular speed in rad/s + put_float_by_index(msg, 8, pitch_speed); // Desired pitch angular speed in rad/s + put_float_by_index(msg, 12, yaw_speed); // Desired yaw angular speed in rad/s + put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 148); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel and send + * @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 + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint32_t time_ms,float roll_speed,float pitch_speed,float yaw_speed,float thrust) +{ + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + + put_uint32_t_by_index(msg, 0, time_ms); // Timestamp in milliseconds since system boot + put_float_by_index(msg, 4, roll_speed); // Desired roll angular speed in rad/s + put_float_by_index(msg, 8, pitch_speed); // Desired pitch angular speed in rad/s + put_float_by_index(msg, 12, yaw_speed); // Desired yaw angular speed in rad/s + put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 + + mavlink_finalize_message_chan_send(msg, chan, 20, 148); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u * @param yaw_speed Desired yaw angular speed in rad/s * @param thrust Collective thrust, normalized to 0 .. 1 */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - mavlink_header_t hdr; - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN ) - payload.time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot - payload.roll_speed = roll_speed; // float:Desired roll angular speed in rad/s - payload.pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s - payload.yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s - payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN; - hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF5, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan_send(chan, msg, time_ms, roll_speed, pitch_speed, yaw_speed, thrust); } #endif + // MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING + /** * @brief Get field time_ms from roll_pitch_yaw_speed_thrust_setpoint message * @@ -135,8 +155,7 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink */ static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_ms(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; - return (uint32_t)(p->time_ms); + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); } /** @@ -146,8 +165,7 @@ static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->roll_speed); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -157,8 +175,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_sp */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->pitch_speed); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -168,8 +185,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_s */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->yaw_speed); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -179,8 +195,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_spe */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->thrust); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -191,5 +206,13 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust( */ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) { - memcpy( roll_pitch_yaw_speed_thrust_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t)); +#if MAVLINK_NEED_BYTE_SWAP + roll_pitch_yaw_speed_thrust_setpoint->time_ms = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_ms(msg); + roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg); +#else + memcpy(roll_pitch_yaw_speed_thrust_setpoint, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index 71aba59306626cb6de7191c1bc21b3c2781c8b06..ab4fb9e3826e16937858f1a7d6357a1ff5e75c04 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -1,21 +1,33 @@ // MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING #define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 57 -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 20 -#define MAVLINK_MSG_57_LEN 20 -#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_KEY 0x38 -#define MAVLINK_MSG_57_KEY 0x38 -typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t +typedef struct __mavlink_roll_pitch_yaw_thrust_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 - float thrust; ///< Collective thrust, normalized to 0 .. 1 - + 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 + float thrust; ///< Collective thrust, normalized to 0 .. 1 } mavlink_roll_pitch_yaw_thrust_setpoint_t; +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 20 +#define MAVLINK_MSG_ID_57_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT { \ + "ROLL_PITCH_YAW_THRUST_SETPOINT", \ + 5, \ + { { "time_ms", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, time_ms) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, yaw) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, thrust) }, \ + } \ +} + + /** * @brief Pack a roll_pitch_yaw_thrust_setpoint message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_ms, float roll, float pitch, float yaw, float thrust) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_uint32_t_by_index(msg, 0, time_ms); // Timestamp in milliseconds since system boot + put_float_by_index(msg, 4, roll); // Desired roll angle in radians + put_float_by_index(msg, 8, pitch); // Desired pitch angle in radians + put_float_by_index(msg, 12, yaw); // Desired yaw angle in radians + put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 141); } /** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t s * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_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, float thrust) +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_ms,float roll,float pitch,float yaw,float thrust) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_uint32_t_by_index(msg, 0, time_ms); // Timestamp in milliseconds since system boot + put_float_by_index(msg, 4, roll); // Desired roll angle in radians + put_float_by_index(msg, 8, pitch); // Desired pitch angle in radians + put_float_by_index(msg, 12, yaw); // Desired yaw angle in radians + put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 141); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel and send + * @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 + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint32_t time_ms,float roll,float pitch,float yaw,float thrust) +{ + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + + put_uint32_t_by_index(msg, 0, time_ms); // Timestamp in milliseconds since system boot + put_float_by_index(msg, 4, roll); // Desired roll angle in radians + put_float_by_index(msg, 8, pitch); // Desired pitch angle in radians + put_float_by_index(msg, 12, yaw); // Desired yaw angle in radians + put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 + + mavlink_finalize_message_chan_send(msg, chan, 20, 141); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a roll_pitch_yaw_thrust_setpoint message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t * @param yaw Desired yaw angle in radians * @param thrust Collective thrust, normalized to 0 .. 1 */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) { - mavlink_header_t hdr; - mavlink_roll_pitch_yaw_thrust_setpoint_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN ) - payload.time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot - payload.roll = roll; // float:Desired roll angle in radians - payload.pitch = pitch; // float:Desired pitch angle in radians - payload.yaw = yaw; // float:Desired yaw angle in radians - payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN; - hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x38, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan_send(chan, msg, time_ms, roll, pitch, yaw, thrust); } #endif + // MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING + /** * @brief Get field time_ms from roll_pitch_yaw_thrust_setpoint message * @@ -135,8 +155,7 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_chann */ static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; - return (uint32_t)(p->time_ms); + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); } /** @@ -146,8 +165,7 @@ static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(co */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -157,8 +175,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const ma */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -168,8 +185,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const m */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -179,8 +195,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mav */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) { - mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; - return (float)(p->thrust); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -191,5 +206,13 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const */ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) { - memcpy( roll_pitch_yaw_thrust_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_thrust_setpoint_t)); +#if MAVLINK_NEED_BYTE_SWAP + roll_pitch_yaw_thrust_setpoint->time_ms = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(msg); + roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg); + roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg); + roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg); + roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg); +#else + memcpy(roll_pitch_yaw_thrust_setpoint, MAVLINK_PAYLOAD(msg), 20); +#endif } 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 0850832420b4312a6c380dc1bd9caf807294c4fd..7d46c97b1823521f82aa4c3a4a948bca71e8b176 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_KEY 0xEA -#define MAVLINK_MSG_54_KEY 0xEA -typedef struct __mavlink_safety_allowed_area_t +typedef struct __mavlink_safety_allowed_area_t { - 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. - + 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; +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 +#define MAVLINK_MSG_ID_54_LEN 25 + + + +#define MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA { \ + "SAFETY_ALLOWED_AREA", \ + 7, \ + { { "p1x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_allowed_area_t, p1x) }, \ + { "p1y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_allowed_area_t, p1y) }, \ + { "p1z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_allowed_area_t, p1z) }, \ + { "p2x", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_allowed_area_t, p2x) }, \ + { "p2y", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_allowed_area_t, p2y) }, \ + { "p2z", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_allowed_area_t, p2z) }, \ + { "frame", MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_allowed_area_t, frame) }, \ + } \ +} + + /** * @brief Pack a safety_allowed_area message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_safety_allowed_area_t * @param p2z z position 2 / Altitude 2 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - 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 + put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 + put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 + put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 + put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 + put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 + put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 + put_uint8_t_by_index(msg, 24, 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. - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 25, 3); } /** - * @brief Pack a safety_allowed_area message + * @brief Pack a safety_allowed_area message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u * @param p2z z position 2 / Altitude 2 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - 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 + put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 + put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 + put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 + put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 + put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 + put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 + put_uint8_t_by_index(msg, 24, 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. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 3); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a safety_allowed_area message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param 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. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +static inline void mavlink_msg_safety_allowed_area_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) +{ + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + + put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 + put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 + put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 + put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 + put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 + put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 + put_uint8_t_by_index(msg, 24, 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_finalize_message_chan_send(msg, chan, 25, 3); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a safety_allowed_area struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a safety_allowed_area message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_safety_allowed_area_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN ) - payload.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. - payload.p1x = p1x; // float:x position 1 / Latitude 1 - payload.p1y = p1y; // float:y position 1 / Longitude 1 - payload.p1z = p1z; // float:z position 1 / Altitude 1 - payload.p2x = p2x; // float:x position 2 / Latitude 2 - payload.p2y = p2y; // float:y position 2 / Longitude 2 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xEA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 25); + mavlink_msg_safety_allowed_area_pack_chan_send(chan, msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); } #endif + // MESSAGE SAFETY_ALLOWED_AREA UNPACKING + /** * @brief Get field frame from safety_allowed_area message * @@ -149,8 +173,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (uint8_t)(p->frame); + return MAVLINK_MSG_RETURN_uint8_t(msg, 24); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (float)(p->p1x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (float)(p->p1y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (float)(p->p1z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (float)(p->p2x); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (float)(p->p2y); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; - return (float)(p->p2z); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( safety_allowed_area, msg->payload, sizeof(mavlink_safety_allowed_area_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); +#else + memcpy(safety_allowed_area, MAVLINK_PAYLOAD(msg), 25); +#endif } 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 2348f95917bb0f301d6b6cec0aeccce3a2610c88..ec187eb406bc0eaf97185133ed89b0e93db6667d 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,25 +1,41 @@ // 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 -#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_KEY 0xF7 -#define MAVLINK_MSG_53_KEY 0xF7 -typedef struct __mavlink_safety_set_allowed_area_t +typedef struct __mavlink_safety_set_allowed_area_t { - 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. - + 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; +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 +#define MAVLINK_MSG_ID_53_LEN 27 + + + +#define MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA { \ + "SAFETY_SET_ALLOWED_AREA", \ + 9, \ + { { "p1x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_safety_set_allowed_area_t, p1x) }, \ + { "p1y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_safety_set_allowed_area_t, p1y) }, \ + { "p1z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_safety_set_allowed_area_t, p1z) }, \ + { "p2x", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_safety_set_allowed_area_t, p2x) }, \ + { "p2y", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_safety_set_allowed_area_t, p2y) }, \ + { "p2z", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_safety_set_allowed_area_t, p2z) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_safety_set_allowed_area_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_safety_set_allowed_area_t, target_component) }, \ + { "frame", MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_safety_set_allowed_area_t, frame) }, \ + } \ +} + + /** * @brief Pack a safety_set_allowed_area message * @param system_id ID of this system @@ -37,26 +53,26 @@ typedef struct __mavlink_safety_set_allowed_area_t * @param p2z z position 2 / Altitude 2 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); + put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 + put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 + put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 + put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 + put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 + put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 + put_uint8_t_by_index(msg, 24, target_system); // System ID + put_uint8_t_by_index(msg, 25, target_component); // Component ID + put_uint8_t_by_index(msg, 26, 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. + + return mavlink_finalize_message(msg, system_id, component_id, 27, 15); } /** - * @brief Pack a safety_set_allowed_area message + * @brief Pack a safety_set_allowed_area message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i * @param p2z z position 2 / Altitude 2 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); + put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 + put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 + put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 + put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 + put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 + put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 + put_uint8_t_by_index(msg, 24, target_system); // System ID + put_uint8_t_by_index(msg, 25, target_component); // Component ID + put_uint8_t_by_index(msg, 26, 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. + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 27, 15); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a safety_set_allowed_area message on a channel and send + * @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 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. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +static inline void mavlink_msg_safety_set_allowed_area_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + + put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 + put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 + put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 + put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 + put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 + put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 + put_uint8_t_by_index(msg, 24, target_system); // System ID + put_uint8_t_by_index(msg, 25, target_component); // Component ID + put_uint8_t_by_index(msg, 26, 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_finalize_message_chan_send(msg, chan, 27, 15); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a safety_set_allowed_area struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a safety_set_allowed_area message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_safety_set_allowed_area_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.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. - payload.p1x = p1x; // float:x position 1 / Latitude 1 - payload.p1y = p1y; // float:y position 1 / Longitude 1 - payload.p1z = p1z; // float:z position 1 / Altitude 1 - payload.p2x = p2x; // float:x position 2 / Latitude 2 - payload.p2y = p2y; // float:y position 2 / Longitude 2 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF7, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 27); + mavlink_msg_safety_set_allowed_area_pack_chan_send(chan, msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); } #endif + // MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING + /** * @brief Get field target_system from safety_set_allowed_area message * @@ -163,8 +191,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 24); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 25); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (uint8_t)(p->frame); + return MAVLINK_MSG_RETURN_uint8_t(msg, 26); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (float)(p->p1x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (float)(p->p1y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (float)(p->p1z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (float)(p->p2x); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (float)(p->p2y); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; - return (float)(p->p2z); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( safety_set_allowed_area, msg->payload, sizeof(mavlink_safety_set_allowed_area_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + 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); +#else + memcpy(safety_set_allowed_area, MAVLINK_PAYLOAD(msg), 27); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index cfa8737d0400e08798686188476fa4641c392756..9c57bdea3eedaaa3d6d2303cf1ed177ca0ce0935 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -1,26 +1,43 @@ // 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 -#define MAVLINK_MSG_ID_SCALED_IMU_KEY 0x1C -#define MAVLINK_MSG_26_KEY 0x1C -typedef struct __mavlink_scaled_imu_t +typedef struct __mavlink_scaled_imu_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) - int16_t xgyro; ///< Angular speed around X axis (millirad /sec) - int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) - int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) - int16_t xmag; ///< X Magnetic field (milli tesla) - int16_t ymag; ///< Y Magnetic field (milli tesla) - int16_t zmag; ///< Z Magnetic field (milli tesla) - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) } mavlink_scaled_imu_t; +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 26 +#define MAVLINK_MSG_ID_26_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ + "SCALED_IMU", \ + 10, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_scaled_imu_t, usec) }, \ + { "xacc", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu_t, xacc) }, \ + { "yacc", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu_t, yacc) }, \ + { "zacc", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu_t, zacc) }, \ + { "xgyro", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu_t, xgyro) }, \ + { "ygyro", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, ygyro) }, \ + { "zgyro", MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, zgyro) }, \ + { "xmag", MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, xmag) }, \ + { "ymag", MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, ymag) }, \ + { "zmag", MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_scaled_imu_t, zmag) }, \ + } \ +} + + /** * @brief Pack a scaled_imu message * @param system_id ID of this system @@ -39,27 +56,27 @@ typedef struct __mavlink_scaled_imu_t * @param zmag Z Magnetic field (milli tesla) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - 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, MAVLINK_MSG_ID_SCALED_IMU_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (mg) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (millirad /sec) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (millirad /sec) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (millirad /sec) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (milli tesla) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (milli tesla) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (milli tesla) + + return mavlink_finalize_message(msg, system_id, component_id, 26, 222); } /** - * @brief Pack a scaled_imu message + * @brief Pack a scaled_imu message on a channel * @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 @@ -76,24 +93,64 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @param zmag Z Magnetic field (milli tesla) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (mg) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (millirad /sec) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (millirad /sec) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (millirad /sec) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (milli tesla) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (milli tesla) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (milli tesla) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 222); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a scaled_imu message on a channel and send + * @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 xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + */ +static inline void mavlink_msg_scaled_imu_pack_chan_send(mavlink_channel_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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - 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, MAVLINK_MSG_ID_SCALED_IMU_LEN); + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (mg) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (millirad /sec) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (millirad /sec) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (millirad /sec) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (milli tesla) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (milli tesla) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (milli tesla) + + mavlink_finalize_message_chan_send(msg, chan, 26, 222); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a scaled_imu struct into a message @@ -108,8 +165,6 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->usec, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a scaled_imu message * @param chan MAVLink channel to send the message @@ -125,44 +180,19 @@ 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 + 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_header_t hdr; - mavlink_scaled_imu_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SCALED_IMU_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.xacc = xacc; // int16_t:X acceleration (mg) - payload.yacc = yacc; // int16_t:Y acceleration (mg) - payload.zacc = zacc; // int16_t:Z acceleration (mg) - payload.xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) - payload.ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) - payload.zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) - payload.xmag = xmag; // int16_t:X Magnetic field (milli tesla) - payload.ymag = ymag; // int16_t:Y Magnetic field (milli tesla) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x1C, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 26); + mavlink_msg_scaled_imu_pack_chan_send(chan, msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); } #endif + // MESSAGE SCALED_IMU UNPACKING + /** * @brief Get field usec from scaled_imu message * @@ -170,8 +200,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -181,8 +210,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->xacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 8); } /** @@ -192,8 +220,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->yacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 10); } /** @@ -203,8 +230,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->zacc); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -214,8 +240,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->xgyro); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -225,8 +250,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->ygyro); + return MAVLINK_MSG_RETURN_int16_t(msg, 16); } /** @@ -236,8 +260,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->zgyro); + return MAVLINK_MSG_RETURN_int16_t(msg, 18); } /** @@ -247,8 +270,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->xmag); + return MAVLINK_MSG_RETURN_int16_t(msg, 20); } /** @@ -258,8 +280,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->ymag); + return MAVLINK_MSG_RETURN_int16_t(msg, 22); } /** @@ -269,8 +290,7 @@ 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) { - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; - return (int16_t)(p->zmag); + return MAVLINK_MSG_RETURN_int16_t(msg, 24); } /** @@ -281,5 +301,18 @@ 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) { - memcpy( scaled_imu, msg->payload, sizeof(mavlink_scaled_imu_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(scaled_imu, MAVLINK_PAYLOAD(msg), 26); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 10e411dc5e391e2a439368b1dd95355c085e7988..ead1fd040df07a1e6f529933bc0d9bf349a5a044 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -1,20 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_SCALED_PRESSURE_KEY 0x16 -#define MAVLINK_MSG_38_KEY 0x16 -typedef struct __mavlink_scaled_pressure_t +typedef struct __mavlink_scaled_pressure_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float press_abs; ///< Absolute pressure (hectopascal) - float press_diff; ///< Differential pressure 1 (hectopascal) - int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) - + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float press_abs; ///< Absolute pressure (hectopascal) + float press_diff; ///< Differential pressure 1 (hectopascal) + int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) } mavlink_scaled_pressure_t; +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 18 +#define MAVLINK_MSG_ID_38_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ + "SCALED_PRESSURE", \ + 4, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_scaled_pressure_t, usec) }, \ + { "press_abs", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ + { "press_diff", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ + { "temperature", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + } \ +} + + /** * @brief Pack a scaled_pressure message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_scaled_pressure_t * @param temperature Temperature measurement (0.01 degrees celsius) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, press_abs); // Absolute pressure (hectopascal) + put_float_by_index(msg, 12, press_diff); // Differential pressure 1 (hectopascal) + put_int16_t_by_index(msg, 16, temperature); // Temperature measurement (0.01 degrees celsius) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 229); } /** - * @brief Pack a scaled_pressure message + * @brief Pack a scaled_pressure message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @param temperature Temperature measurement (0.01 degrees celsius) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, press_abs); // Absolute pressure (hectopascal) + put_float_by_index(msg, 12, press_diff); // Differential pressure 1 (hectopascal) + put_int16_t_by_index(msg, 16, temperature); // Temperature measurement (0.01 degrees celsius) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 229); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a scaled_pressure message on a channel and send + * @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 press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + */ +static inline void mavlink_msg_scaled_pressure_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float press_abs,float press_diff,int16_t temperature) { - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - 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) + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, press_abs); // Absolute pressure (hectopascal) + put_float_by_index(msg, 12, press_diff); // Differential pressure 1 (hectopascal) + put_int16_t_by_index(msg, 16, temperature); // Temperature measurement (0.01 degrees celsius) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 229); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a scaled_pressure struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->usec, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a scaled_pressure message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + 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_header_t hdr; - mavlink_scaled_pressure_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - payload.press_abs = press_abs; // float:Absolute pressure (hectopascal) - payload.press_diff = press_diff; // float:Differential pressure 1 (hectopascal) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x16, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_scaled_pressure_pack_chan_send(chan, msg, usec, press_abs, press_diff, temperature); } #endif + // MESSAGE SCALED_PRESSURE UNPACKING + /** * @brief Get field usec from scaled_pressure message * @@ -128,8 +146,7 @@ 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) { - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; - return (float)(p->press_abs); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; - return (float)(p->press_diff); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; - return (int16_t)(p->temperature); + return MAVLINK_MSG_RETURN_int16_t(msg, 16); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( scaled_pressure, msg->payload, sizeof(mavlink_scaled_pressure_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(scaled_pressure, MAVLINK_PAYLOAD(msg), 18); +#endif } 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 2897698c384920730db678c0a384a0cae4ed9e1b..583b08bd862bd18c25d64947da105f560ceaea8a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -1,24 +1,39 @@ // 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 -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_KEY 0xEA -#define MAVLINK_MSG_37_KEY 0xEA -typedef struct __mavlink_servo_output_raw_t +typedef struct __mavlink_servo_output_raw_t { - uint16_t servo1_raw; ///< Servo output 1 value, in microseconds - uint16_t servo2_raw; ///< Servo output 2 value, in microseconds - uint16_t servo3_raw; ///< Servo output 3 value, in microseconds - uint16_t servo4_raw; ///< Servo output 4 value, in microseconds - uint16_t servo5_raw; ///< Servo output 5 value, in microseconds - uint16_t servo6_raw; ///< Servo output 6 value, in microseconds - uint16_t servo7_raw; ///< Servo output 7 value, in microseconds - uint16_t servo8_raw; ///< Servo output 8 value, in microseconds - + uint16_t servo1_raw; ///< Servo output 1 value, in microseconds + uint16_t servo2_raw; ///< Servo output 2 value, in microseconds + uint16_t servo3_raw; ///< Servo output 3 value, in microseconds + uint16_t servo4_raw; ///< Servo output 4 value, in microseconds + uint16_t servo5_raw; ///< Servo output 5 value, in microseconds + uint16_t servo6_raw; ///< Servo output 6 value, in microseconds + uint16_t servo7_raw; ///< Servo output 7 value, in microseconds + uint16_t servo8_raw; ///< Servo output 8 value, in microseconds } mavlink_servo_output_raw_t; +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 16 +#define MAVLINK_MSG_ID_37_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ + "SERVO_OUTPUT_RAW", \ + 8, \ + { { "servo1_raw", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ + { "servo2_raw", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \ + { "servo3_raw", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \ + { "servo4_raw", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \ + { "servo5_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \ + { "servo6_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ + { "servo7_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ + { "servo8_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + } \ +} + + /** * @brief Pack a servo_output_raw message * @param system_id ID of this system @@ -35,25 +50,25 @@ typedef struct __mavlink_servo_output_raw_t * @param servo8_raw Servo output 8 value, in microseconds * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - 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 + put_uint16_t_by_index(msg, 0, servo1_raw); // Servo output 1 value, in microseconds + put_uint16_t_by_index(msg, 2, servo2_raw); // Servo output 2 value, in microseconds + put_uint16_t_by_index(msg, 4, servo3_raw); // Servo output 3 value, in microseconds + put_uint16_t_by_index(msg, 6, servo4_raw); // Servo output 4 value, in microseconds + put_uint16_t_by_index(msg, 8, servo5_raw); // Servo output 5 value, in microseconds + put_uint16_t_by_index(msg, 10, servo6_raw); // Servo output 6 value, in microseconds + put_uint16_t_by_index(msg, 12, servo7_raw); // Servo output 7 value, in microseconds + put_uint16_t_by_index(msg, 14, servo8_raw); // Servo output 8 value, in microseconds - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 16, 215); } /** - * @brief Pack a servo_output_raw message + * @brief Pack a servo_output_raw message on a channel * @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 @@ -68,22 +83,58 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @param servo8_raw Servo output 8 value, in microseconds * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + + put_uint16_t_by_index(msg, 0, servo1_raw); // Servo output 1 value, in microseconds + put_uint16_t_by_index(msg, 2, servo2_raw); // Servo output 2 value, in microseconds + put_uint16_t_by_index(msg, 4, servo3_raw); // Servo output 3 value, in microseconds + put_uint16_t_by_index(msg, 6, servo4_raw); // Servo output 4 value, in microseconds + put_uint16_t_by_index(msg, 8, servo5_raw); // Servo output 5 value, in microseconds + put_uint16_t_by_index(msg, 10, servo6_raw); // Servo output 6 value, in microseconds + put_uint16_t_by_index(msg, 12, servo7_raw); // Servo output 7 value, in microseconds + put_uint16_t_by_index(msg, 14, servo8_raw); // Servo output 8 value, in microseconds + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 215); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a servo_output_raw message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + */ +static inline void mavlink_msg_servo_output_raw_pack_chan_send(mavlink_channel_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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - 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 + put_uint16_t_by_index(msg, 0, servo1_raw); // Servo output 1 value, in microseconds + put_uint16_t_by_index(msg, 2, servo2_raw); // Servo output 2 value, in microseconds + put_uint16_t_by_index(msg, 4, servo3_raw); // Servo output 3 value, in microseconds + put_uint16_t_by_index(msg, 6, servo4_raw); // Servo output 4 value, in microseconds + put_uint16_t_by_index(msg, 8, servo5_raw); // Servo output 5 value, in microseconds + put_uint16_t_by_index(msg, 10, servo6_raw); // Servo output 6 value, in microseconds + put_uint16_t_by_index(msg, 12, servo7_raw); // Servo output 7 value, in microseconds + put_uint16_t_by_index(msg, 14, servo8_raw); // Servo output 8 value, in microseconds - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); + mavlink_finalize_message_chan_send(msg, chan, 16, 215); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a servo_output_raw struct into a message @@ -98,8 +149,6 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a servo_output_raw message * @param chan MAVLink channel to send the message @@ -113,42 +162,19 @@ 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 + 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_header_t hdr; - mavlink_servo_output_raw_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN ) - payload.servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds - payload.servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds - payload.servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds - payload.servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds - payload.servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds - payload.servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds - payload.servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xEA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 16); + mavlink_msg_servo_output_raw_pack_chan_send(chan, msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); } #endif + // MESSAGE SERVO_OUTPUT_RAW UNPACKING + /** * @brief Get field servo1_raw from servo_output_raw message * @@ -156,8 +182,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo1_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -167,8 +192,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo2_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 2); } /** @@ -178,8 +202,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo3_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -189,8 +212,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo4_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -200,8 +222,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo5_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -211,8 +232,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo6_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 10); } /** @@ -222,8 +242,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo7_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -233,8 +252,7 @@ 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) { - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; - return (uint16_t)(p->servo8_raw); + return MAVLINK_MSG_RETURN_uint16_t(msg, 14); } /** @@ -245,5 +263,16 @@ 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) { - memcpy( servo_output_raw, msg->payload, sizeof(mavlink_servo_output_raw_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(servo_output_raw, MAVLINK_PAYLOAD(msg), 16); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index d24cfb5ca0481bfea11ce10f8992cd4408b23992..a6b1a645d2be1f5c18e4947fa44732c173e28de4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -1,16 +1,11 @@ // 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 -#define MAVLINK_MSG_ID_SET_ALTITUDE_KEY 0x12 -#define MAVLINK_MSG_65_KEY 0x12 -typedef struct __mavlink_set_altitude_t +typedef struct __mavlink_set_altitude_t { - uint32_t mode; ///< The new altitude in meters - 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; /** @@ -23,19 +18,19 @@ typedef struct __mavlink_set_altitude_t * @param mode The new altitude in meters * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters + put_uint32_t_by_index(mode, 0, msg->payload); // The new altitude in meters + put_uint8_t_by_index(target, 4, msg->payload); // The system setting the altitude - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 5, 56); } /** - * @brief Pack a set_altitude message + * @brief Pack a set_altitude message on a channel * @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 @@ -44,16 +39,40 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t * @param mode The new altitude in meters * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + + put_uint32_t_by_index(mode, 0, msg->payload); // The new altitude in meters + put_uint8_t_by_index(target, 4, msg->payload); // The system setting the altitude + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 56); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_altitude message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the altitude + * @param mode The new altitude in meters + */ +static inline void mavlink_msg_set_altitude_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint32_t mode) { - mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters + put_uint32_t_by_index(mode, 0, msg->payload); // The new altitude in meters + put_uint8_t_by_index(target, 4, msg->payload); // The system setting the altitude - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 5, 56); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_altitude struct into a message @@ -68,8 +87,6 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ return mavlink_msg_set_altitude_pack(system_id, component_id, msg, set_altitude->target, set_altitude->mode); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_altitude message * @param chan MAVLink channel to send the message @@ -77,36 +94,19 @@ 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 + static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) { - mavlink_header_t hdr; - mavlink_set_altitude_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN ) - payload.target = target; // uint8_t:The system setting the altitude - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x12, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 5); + mavlink_msg_set_altitude_pack_chan_send(chan, msg, target, mode); } #endif + // MESSAGE SET_ALTITUDE UNPACKING + /** * @brief Get field target from set_altitude message * @@ -114,8 +114,7 @@ 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) { - mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -125,8 +124,7 @@ 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) { - mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; - return (uint32_t)(p->mode); + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); } /** @@ -137,5 +135,10 @@ 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) { - memcpy( set_altitude, msg->payload, sizeof(mavlink_set_altitude_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); + set_altitude->target = mavlink_msg_set_altitude_get_target(msg); +#else + memcpy(set_altitude, msg->payload, 5); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h index c8e4bd26f4d35157efdfa367b9b73f1df820ad9d..d7d6f25051bb885a521fb4e10637ce72b59a166a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h @@ -1,18 +1,27 @@ // MESSAGE SET_FLIGHT_MODE PACKING #define MAVLINK_MSG_ID_SET_FLIGHT_MODE 12 -#define MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN 2 -#define MAVLINK_MSG_12_LEN 2 -#define MAVLINK_MSG_ID_SET_FLIGHT_MODE_KEY 0xAC -#define MAVLINK_MSG_12_KEY 0xAC -typedef struct __mavlink_set_flight_mode_t +typedef struct __mavlink_set_flight_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t flight_mode; ///< The new navigation mode - + uint8_t target; ///< The system setting the mode + uint8_t flight_mode; ///< The new navigation mode } mavlink_set_flight_mode_t; +#define MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN 2 +#define MAVLINK_MSG_ID_12_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE { \ + "SET_FLIGHT_MODE", \ + 2, \ + { { "target", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_flight_mode_t, target) }, \ + { "flight_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_flight_mode_t, flight_mode) }, \ + } \ +} + + /** * @brief Pack a set_flight_mode message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_set_flight_mode_t * @param flight_mode The new navigation mode * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_flight_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t flight_mode) +static inline uint16_t mavlink_msg_set_flight_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, uint8_t flight_mode) { - mavlink_set_flight_mode_t *p = (mavlink_set_flight_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; - p->target = target; // uint8_t:The system setting the mode - p->flight_mode = flight_mode; // uint8_t:The new navigation mode + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, flight_mode); // The new navigation mode - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 194); } /** - * @brief Pack a set_flight_mode message + * @brief Pack a set_flight_mode message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_set_flight_mode_pack(uint8_t system_id, uint8 * @param flight_mode The new navigation mode * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_flight_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t flight_mode) +static inline uint16_t mavlink_msg_set_flight_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t flight_mode) +{ + msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; + + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, flight_mode); // The new navigation mode + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 194); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_flight_mode message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the mode + * @param flight_mode The new navigation mode + */ +static inline void mavlink_msg_set_flight_mode_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t flight_mode) { - mavlink_set_flight_mode_t *p = (mavlink_set_flight_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; - p->target = target; // uint8_t:The system setting the mode - p->flight_mode = flight_mode; // uint8_t:The new navigation mode + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, flight_mode); // The new navigation mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 194); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_flight_mode struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_set_flight_mode_encode(uint8_t system_id, uin return mavlink_msg_set_flight_mode_pack(system_id, component_id, msg, set_flight_mode->target, set_flight_mode->flight_mode); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_flight_mode message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ static inline uint16_t mavlink_msg_set_flight_mode_encode(uint8_t system_id, uin * @param target The system setting the mode * @param flight_mode The new navigation mode */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_set_flight_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t flight_mode) { - mavlink_header_t hdr; - mavlink_set_flight_mode_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN ) - payload.target = target; // uint8_t:The system setting the mode - payload.flight_mode = flight_mode; // uint8_t:The new navigation mode - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN; - hdr.msgid = MAVLINK_MSG_ID_SET_FLIGHT_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xAC, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_set_flight_mode_pack_chan_send(chan, msg, target, flight_mode); } #endif + // MESSAGE SET_FLIGHT_MODE UNPACKING + /** * @brief Get field target from set_flight_mode message * @@ -114,8 +128,7 @@ static inline void mavlink_msg_set_flight_mode_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_flight_mode_get_target(const mavlink_message_t* msg) { - mavlink_set_flight_mode_t *p = (mavlink_set_flight_mode_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +138,7 @@ static inline uint8_t mavlink_msg_set_flight_mode_get_target(const mavlink_messa */ static inline uint8_t mavlink_msg_set_flight_mode_get_flight_mode(const mavlink_message_t* msg) { - mavlink_set_flight_mode_t *p = (mavlink_set_flight_mode_t *)&msg->payload[0]; - return (uint8_t)(p->flight_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +149,10 @@ static inline uint8_t mavlink_msg_set_flight_mode_get_flight_mode(const mavlink_ */ static inline void mavlink_msg_set_flight_mode_decode(const mavlink_message_t* msg, mavlink_set_flight_mode_t* set_flight_mode) { - memcpy( set_flight_mode, msg->payload, sizeof(mavlink_set_flight_mode_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_flight_mode->target = mavlink_msg_set_flight_mode_get_target(msg); + set_flight_mode->flight_mode = mavlink_msg_set_flight_mode_get_flight_mode(msg); +#else + memcpy(set_flight_mode, MAVLINK_PAYLOAD(msg), 2); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index 065710677b57846792c754cde0495792cbc25e3a..9f80f46344904d30e5faebe9175d0f1583decfb3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -1,18 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_SET_MODE_KEY 0xF9 -#define MAVLINK_MSG_11_KEY 0xF9 -typedef struct __mavlink_set_mode_t +typedef struct __mavlink_set_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t mode; ///< The new mode - + uint8_t target; ///< The system setting the mode + uint8_t mode; ///< The new mode } mavlink_set_mode_t; +#define MAVLINK_MSG_ID_SET_MODE_LEN 2 +#define MAVLINK_MSG_ID_11_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_SET_MODE { \ + "SET_MODE", \ + 2, \ + { { "target", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_mode_t, target) }, \ + { "mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_mode_t, mode) }, \ + } \ +} + + /** * @brief Pack a set_mode message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_set_mode_t * @param mode The new mode * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, mode); // The new mode - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 186); } /** - * @brief Pack a set_mode message + * @brief Pack a set_mode message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp * @param mode The new mode * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SET_MODE; + + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, mode); // The new mode + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 186); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_mode message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the mode + * @param mode The new mode + */ +static inline void mavlink_msg_set_mode_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t mode) { - mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, mode); // The new mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 186); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_mode struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target, set_mode->mode); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_mode message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { - mavlink_header_t hdr; - mavlink_set_mode_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_MODE_LEN ) - payload.target = target; // uint8_t:The system setting the mode - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xF9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_set_mode_pack_chan_send(chan, msg, target, mode); } #endif + // MESSAGE SET_MODE UNPACKING + /** * @brief Get field target from set_mode message * @@ -114,8 +128,7 @@ 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) { - mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; - return (uint8_t)(p->mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( set_mode, msg->payload, sizeof(mavlink_set_mode_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_mode->target = mavlink_msg_set_mode_get_target(msg); + set_mode->mode = mavlink_msg_set_mode_get_mode(msg); +#else + memcpy(set_mode, MAVLINK_PAYLOAD(msg), 2); +#endif } 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 a029ec26de0f90197a644e0fe84f5f4dbdd4c2c7..ff49bb48e6e044b76c02e71353299708c0b819f7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -1,16 +1,11 @@ // 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 -#define MAVLINK_MSG_ID_SET_NAV_MODE_KEY 0x85 -#define MAVLINK_MSG_12_KEY 0x85 -typedef struct __mavlink_set_nav_mode_t +typedef struct __mavlink_set_nav_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t nav_mode; ///< The new navigation mode - + uint8_t target; ///< The system setting the mode + uint8_t nav_mode; ///< The new navigation mode } mavlink_set_nav_mode_t; /** @@ -23,19 +18,19 @@ typedef struct __mavlink_set_nav_mode_t * @param nav_mode The new navigation mode * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode + put_uint8_t_by_index(target, 0, msg->payload); // The system setting the mode + put_uint8_t_by_index(nav_mode, 1, msg->payload); // The new navigation mode - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 134); } /** - * @brief Pack a set_nav_mode message + * @brief Pack a set_nav_mode message on a channel * @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 @@ -44,16 +39,40 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t * @param nav_mode The new navigation mode * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + + put_uint8_t_by_index(target, 0, msg->payload); // The system setting the mode + put_uint8_t_by_index(nav_mode, 1, msg->payload); // The new navigation mode + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 134); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_nav_mode message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the mode + * @param nav_mode The new navigation mode + */ +static inline void mavlink_msg_set_nav_mode_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t nav_mode) { - mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode + put_uint8_t_by_index(target, 0, msg->payload); // The system setting the mode + put_uint8_t_by_index(nav_mode, 1, msg->payload); // The new navigation mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 134); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_nav_mode struct into a message @@ -68,8 +87,6 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ return mavlink_msg_set_nav_mode_pack(system_id, component_id, msg, set_nav_mode->target, set_nav_mode->nav_mode); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_nav_mode message * @param chan MAVLink channel to send the message @@ -77,36 +94,19 @@ 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 + static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) { - mavlink_header_t hdr; - mavlink_set_nav_mode_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN ) - payload.target = target; // uint8_t:The system setting the mode - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x85, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_set_nav_mode_pack_chan_send(chan, msg, target, nav_mode); } #endif + // MESSAGE SET_NAV_MODE UNPACKING + /** * @brief Get field target from set_nav_mode message * @@ -114,8 +114,7 @@ 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) { - mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +124,7 @@ 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) { - mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; - return (uint8_t)(p->nav_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +135,10 @@ 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) { - memcpy( set_nav_mode, msg->payload, sizeof(mavlink_set_nav_mode_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(set_nav_mode, msg->payload, 2); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index fab1fbd45057f7912db53fc5db0927f4765a1258..ad07f2bacb9b23fbf7252637ae5be39b1b397fb5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -1,22 +1,35 @@ // MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING #define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 56 -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18 -#define MAVLINK_MSG_56_LEN 18 -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_KEY 0x74 -#define MAVLINK_MSG_56_KEY 0x74 -typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t +typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_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 - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_set_roll_pitch_yaw_speed_thrust_t; +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18 +#define MAVLINK_MSG_ID_56_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST { \ + "SET_ROLL_PITCH_YAW_SPEED_THRUST", \ + 6, \ + { { "roll_speed", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, roll_speed) }, \ + { "pitch_speed", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, pitch_speed) }, \ + { "yaw_speed", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, yaw_speed) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, thrust) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_component) }, \ + } \ +} + + /** * @brief Pack a set_roll_pitch_yaw_speed_thrust message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - 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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_float_by_index(msg, 0, roll_speed); // Desired roll angular speed in rad/s + put_float_by_index(msg, 4, pitch_speed); // Desired pitch angular speed in rad/s + put_float_by_index(msg, 8, yaw_speed); // Desired yaw angular speed in rad/s + put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 24); } /** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float roll_speed,float pitch_speed,float yaw_speed,float thrust) +{ + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + + put_float_by_index(msg, 0, roll_speed); // Desired roll angular speed in rad/s + put_float_by_index(msg, 4, pitch_speed); // Desired pitch angular speed in rad/s + put_float_by_index(msg, 8, yaw_speed); // Desired yaw angular speed in rad/s + put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 24); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float roll_speed,float pitch_speed,float yaw_speed,float thrust) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - 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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_float_by_index(msg, 0, roll_speed); // Desired roll angular speed in rad/s + put_float_by_index(msg, 4, pitch_speed); // Desired pitch angular speed in rad/s + put_float_by_index(msg, 8, yaw_speed); // Desired yaw angular speed in rad/s + put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 24); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_roll_pitch_yaw_speed_thrust message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ * @param yaw_speed Desired yaw angular speed in rad/s * @param thrust Collective thrust, normalized to 0 .. 1 */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - mavlink_header_t hdr; - mavlink_set_roll_pitch_yaw_speed_thrust_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.roll_speed = roll_speed; // float:Desired roll angular speed in rad/s - payload.pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s - payload.yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s - payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN; - hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x74, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan_send(chan, msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust); } #endif + // MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING + /** * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message * @@ -142,8 +164,7 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -153,8 +174,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_sys */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -164,8 +184,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_com */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; - return (float)(p->roll_speed); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -175,8 +194,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(c */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; - return (float)(p->pitch_speed); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -186,8 +204,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed( */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; - return (float)(p->yaw_speed); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -197,8 +214,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(co */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; - return (float)(p->thrust); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -209,5 +225,14 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const */ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) { - memcpy( set_roll_pitch_yaw_speed_thrust, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_thrust_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg); + set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg); + set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg); + set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg); + set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg); + set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg); +#else + memcpy(set_roll_pitch_yaw_speed_thrust, MAVLINK_PAYLOAD(msg), 18); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index f2a7ca6cac03087b7e1d374b8fa7e5d529944622..22a57adde2f1a081ad16a0740b5bbf7809d81010 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -1,22 +1,35 @@ // MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING #define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 55 -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18 -#define MAVLINK_MSG_55_LEN 18 -#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_KEY 0xA1 -#define MAVLINK_MSG_55_KEY 0xA1 -typedef struct __mavlink_set_roll_pitch_yaw_thrust_t +typedef struct __mavlink_set_roll_pitch_yaw_thrust_t { - float roll; ///< Desired roll angle in radians - float pitch; ///< Desired pitch angle in radians - float yaw; ///< Desired yaw angle in radians - float thrust; ///< Collective thrust, normalized to 0 .. 1 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_set_roll_pitch_yaw_thrust_t; +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18 +#define MAVLINK_MSG_ID_55_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST { \ + "SET_ROLL_PITCH_YAW_THRUST", \ + 6, \ + { { "roll", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, yaw) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, thrust) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_component) }, \ + } \ +} + + /** * @brief Pack a set_roll_pitch_yaw_thrust message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_set_roll_pitch_yaw_thrust_t * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - 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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_float_by_index(msg, 0, roll); // Desired roll angle in radians + put_float_by_index(msg, 4, pitch); // Desired pitch angle in radians + put_float_by_index(msg, 8, yaw); // Desired yaw angle in radians + put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 100); } /** - * @brief Pack a set_roll_pitch_yaw_thrust message + * @brief Pack a set_roll_pitch_yaw_thrust message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system * @param thrust Collective thrust, normalized to 0 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float roll,float pitch,float yaw,float thrust) +{ + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + + put_float_by_index(msg, 0, roll); // Desired roll angle in radians + put_float_by_index(msg, 4, pitch); // Desired pitch angle in radians + put_float_by_index(msg, 8, yaw); // Desired yaw angle in radians + put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 100); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float roll,float pitch,float yaw,float thrust) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - 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 - p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + put_float_by_index(msg, 0, roll); // Desired roll angle in radians + put_float_by_index(msg, 4, pitch); // Desired pitch angle in radians + put_float_by_index(msg, 8, yaw); // Desired yaw angle in radians + put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 100); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_roll_pitch_yaw_thrust struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst * @param yaw Desired yaw angle in radians * @param thrust Collective thrust, normalized to 0 .. 1 */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) { - mavlink_header_t hdr; - mavlink_set_roll_pitch_yaw_thrust_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.roll = roll; // float:Desired roll angle in radians - payload.pitch = pitch; // float:Desired pitch angle in radians - payload.yaw = yaw; // float:Desired yaw angle in radians - payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN; - hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA1, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan_send(chan, msg, target_system, target_component, roll, pitch, yaw, thrust); } #endif + // MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING + /** * @brief Get field target_system from set_roll_pitch_yaw_thrust message * @@ -142,8 +164,7 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -153,8 +174,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(co */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -164,8 +184,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -175,8 +194,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -186,8 +204,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlin */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -197,8 +214,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_ */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) { - mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; - return (float)(p->thrust); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -209,5 +225,14 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavli */ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) { - memcpy( set_roll_pitch_yaw_thrust, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_thrust_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg); + set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg); + set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg); + set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg); + set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg); + set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg); +#else + memcpy(set_roll_pitch_yaw_thrust, MAVLINK_PAYLOAD(msg), 18); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h index 17b5ecccc7e46323181a0b237e9fe34236d23bab..fb54cc1294112ab33de5b655c05cc6b6da84a48e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h @@ -1,18 +1,27 @@ // MESSAGE SET_SAFETY_MODE PACKING #define MAVLINK_MSG_ID_SET_SAFETY_MODE 13 -#define MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN 2 -#define MAVLINK_MSG_13_LEN 2 -#define MAVLINK_MSG_ID_SET_SAFETY_MODE_KEY 0x10 -#define MAVLINK_MSG_13_KEY 0x10 -typedef struct __mavlink_set_safety_mode_t +typedef struct __mavlink_set_safety_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t safety_mode; ///< The new safety mode. The MAV will reject some mode changes during flight. - + uint8_t target; ///< The system setting the mode + uint8_t safety_mode; ///< The new safety mode. The MAV will reject some mode changes during flight. } mavlink_set_safety_mode_t; +#define MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN 2 +#define MAVLINK_MSG_ID_13_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE { \ + "SET_SAFETY_MODE", \ + 2, \ + { { "target", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_safety_mode_t, target) }, \ + { "safety_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_safety_mode_t, safety_mode) }, \ + } \ +} + + /** * @brief Pack a set_safety_mode message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_set_safety_mode_t * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_safety_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t safety_mode) +static inline uint16_t mavlink_msg_set_safety_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, uint8_t safety_mode) { - mavlink_set_safety_mode_t *p = (mavlink_set_safety_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; - p->target = target; // uint8_t:The system setting the mode - p->safety_mode = safety_mode; // uint8_t:The new safety mode. The MAV will reject some mode changes during flight. + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, safety_mode); // The new safety mode. The MAV will reject some mode changes during flight. - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 8); } /** - * @brief Pack a set_safety_mode message + * @brief Pack a set_safety_mode message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_set_safety_mode_pack(uint8_t system_id, uint8 * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_safety_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t safety_mode) +static inline uint16_t mavlink_msg_set_safety_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t safety_mode) +{ + msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; + + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, safety_mode); // The new safety mode. The MAV will reject some mode changes during flight. + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 8); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_safety_mode message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the mode + * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. + */ +static inline void mavlink_msg_set_safety_mode_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t safety_mode) { - mavlink_set_safety_mode_t *p = (mavlink_set_safety_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; - p->target = target; // uint8_t:The system setting the mode - p->safety_mode = safety_mode; // uint8_t:The new safety mode. The MAV will reject some mode changes during flight. + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, safety_mode); // The new safety mode. The MAV will reject some mode changes during flight. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 8); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_safety_mode struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_set_safety_mode_encode(uint8_t system_id, uin return mavlink_msg_set_safety_mode_pack(system_id, component_id, msg, set_safety_mode->target, set_safety_mode->safety_mode); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_safety_mode message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ static inline uint16_t mavlink_msg_set_safety_mode_encode(uint8_t system_id, uin * @param target The system setting the mode * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_set_safety_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t safety_mode) { - mavlink_header_t hdr; - mavlink_set_safety_mode_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN ) - payload.target = target; // uint8_t:The system setting the mode - payload.safety_mode = safety_mode; // uint8_t:The new safety mode. The MAV will reject some mode changes during flight. - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN; - hdr.msgid = MAVLINK_MSG_ID_SET_SAFETY_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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x10, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_set_safety_mode_pack_chan_send(chan, msg, target, safety_mode); } #endif + // MESSAGE SET_SAFETY_MODE UNPACKING + /** * @brief Get field target from set_safety_mode message * @@ -114,8 +128,7 @@ static inline void mavlink_msg_set_safety_mode_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_safety_mode_get_target(const mavlink_message_t* msg) { - mavlink_set_safety_mode_t *p = (mavlink_set_safety_mode_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +138,7 @@ static inline uint8_t mavlink_msg_set_safety_mode_get_target(const mavlink_messa */ static inline uint8_t mavlink_msg_set_safety_mode_get_safety_mode(const mavlink_message_t* msg) { - mavlink_set_safety_mode_t *p = (mavlink_set_safety_mode_t *)&msg->payload[0]; - return (uint8_t)(p->safety_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +149,10 @@ static inline uint8_t mavlink_msg_set_safety_mode_get_safety_mode(const mavlink_ */ static inline void mavlink_msg_set_safety_mode_decode(const mavlink_message_t* msg, mavlink_set_safety_mode_t* set_safety_mode) { - memcpy( set_safety_mode, msg->payload, sizeof(mavlink_set_safety_mode_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_safety_mode->target = mavlink_msg_set_safety_mode_get_target(msg); + set_safety_mode->safety_mode = mavlink_msg_set_safety_mode_get_safety_mode(msg); +#else + memcpy(set_safety_mode, MAVLINK_PAYLOAD(msg), 2); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index ab48856674e0c077ee7744812ec9cf9e16570d17..afdcf9fd5837f395f3404e001b38367ccb84ce1b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -1,25 +1,41 @@ // 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 -#define MAVLINK_MSG_ID_STATE_CORRECTION_KEY 0xB9 -#define MAVLINK_MSG_64_KEY 0xB9 -typedef struct __mavlink_state_correction_t +typedef struct __mavlink_state_correction_t { - float xErr; ///< x position error - float yErr; ///< y position error - float zErr; ///< z position error - float rollErr; ///< roll error (radians) - float pitchErr; ///< pitch error (radians) - float yawErr; ///< yaw error (radians) - float vxErr; ///< x velocity - float vyErr; ///< y velocity - float vzErr; ///< z velocity - + float xErr; ///< x position error + float yErr; ///< y position error + float zErr; ///< z position error + float rollErr; ///< roll error (radians) + float pitchErr; ///< pitch error (radians) + float yawErr; ///< yaw error (radians) + float vxErr; ///< x velocity + float vyErr; ///< y velocity + float vzErr; ///< z velocity } mavlink_state_correction_t; +#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 +#define MAVLINK_MSG_ID_64_LEN 36 + + + +#define MAVLINK_MESSAGE_INFO_STATE_CORRECTION { \ + "STATE_CORRECTION", \ + 9, \ + { { "xErr", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_state_correction_t, xErr) }, \ + { "yErr", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_state_correction_t, yErr) }, \ + { "zErr", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_state_correction_t, zErr) }, \ + { "rollErr", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_state_correction_t, rollErr) }, \ + { "pitchErr", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_state_correction_t, pitchErr) }, \ + { "yawErr", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_state_correction_t, yawErr) }, \ + { "vxErr", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_state_correction_t, vxErr) }, \ + { "vyErr", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_state_correction_t, vyErr) }, \ + { "vzErr", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_state_correction_t, vzErr) }, \ + } \ +} + + /** * @brief Pack a state_correction message * @param system_id ID of this system @@ -37,26 +53,26 @@ typedef struct __mavlink_state_correction_t * @param vzErr z velocity * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - 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, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); + put_float_by_index(msg, 0, xErr); // x position error + put_float_by_index(msg, 4, yErr); // y position error + put_float_by_index(msg, 8, zErr); // z position error + put_float_by_index(msg, 12, rollErr); // roll error (radians) + put_float_by_index(msg, 16, pitchErr); // pitch error (radians) + put_float_by_index(msg, 20, yawErr); // yaw error (radians) + put_float_by_index(msg, 24, vxErr); // x velocity + put_float_by_index(msg, 28, vyErr); // y velocity + put_float_by_index(msg, 32, vzErr); // z velocity + + return mavlink_finalize_message(msg, system_id, component_id, 36, 130); } /** - * @brief Pack a state_correction message + * @brief Pack a state_correction message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint * @param vzErr z velocity * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - 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, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); + put_float_by_index(msg, 0, xErr); // x position error + put_float_by_index(msg, 4, yErr); // y position error + put_float_by_index(msg, 8, zErr); // z position error + put_float_by_index(msg, 12, rollErr); // roll error (radians) + put_float_by_index(msg, 16, pitchErr); // pitch error (radians) + put_float_by_index(msg, 20, yawErr); // yaw error (radians) + put_float_by_index(msg, 24, vxErr); // x velocity + put_float_by_index(msg, 28, vyErr); // y velocity + put_float_by_index(msg, 32, vzErr); // z velocity + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 130); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a state_correction message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param xErr x position error + * @param yErr y position error + * @param zErr z position error + * @param rollErr roll error (radians) + * @param pitchErr pitch error (radians) + * @param yawErr yaw error (radians) + * @param vxErr x velocity + * @param vyErr y velocity + * @param vzErr z velocity + */ +static inline void mavlink_msg_state_correction_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float xErr,float yErr,float zErr,float rollErr,float pitchErr,float yawErr,float vxErr,float vyErr,float vzErr) +{ + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + + put_float_by_index(msg, 0, xErr); // x position error + put_float_by_index(msg, 4, yErr); // y position error + put_float_by_index(msg, 8, zErr); // z position error + put_float_by_index(msg, 12, rollErr); // roll error (radians) + put_float_by_index(msg, 16, pitchErr); // pitch error (radians) + put_float_by_index(msg, 20, yawErr); // yaw error (radians) + put_float_by_index(msg, 24, vxErr); // x velocity + put_float_by_index(msg, 28, vyErr); // y velocity + put_float_by_index(msg, 32, vzErr); // z velocity + + mavlink_finalize_message_chan_send(msg, chan, 36, 130); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a state_correction struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a state_correction message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_state_correction_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN ) - payload.xErr = xErr; // float:x position error - payload.yErr = yErr; // float:y position error - payload.zErr = zErr; // float:z position error - payload.rollErr = rollErr; // float:roll error (radians) - payload.pitchErr = pitchErr; // float:pitch error (radians) - payload.yawErr = yawErr; // float:yaw error (radians) - payload.vxErr = vxErr; // float:x velocity - payload.vyErr = vyErr; // float:y velocity - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xB9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 36); + mavlink_msg_state_correction_pack_chan_send(chan, msg, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr); } #endif + // MESSAGE STATE_CORRECTION UNPACKING + /** * @brief Get field xErr from state_correction message * @@ -163,8 +191,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->xErr); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->yErr); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->zErr); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->rollErr); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->pitchErr); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->yawErr); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->vxErr); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->vyErr); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; - return (float)(p->vzErr); + return MAVLINK_MSG_RETURN_float(msg, 32); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( state_correction, msg->payload, sizeof(mavlink_state_correction_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(state_correction, MAVLINK_PAYLOAD(msg), 36); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index f05df5b0234776be1ce5ba0d161209995607d72c..23b3c0d34a3a8ebef906a59de643734ed652da48 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -1,19 +1,27 @@ // MESSAGE STATUSTEXT PACKING #define MAVLINK_MSG_ID_STATUSTEXT 254 -#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 -#define MAVLINK_MSG_254_LEN 51 -#define MAVLINK_MSG_ID_STATUSTEXT_KEY 0x90 -#define MAVLINK_MSG_254_KEY 0x90 -typedef struct __mavlink_statustext_t +typedef struct __mavlink_statustext_t { - uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault - char text[50]; ///< Status text message, without null termination character - + uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault + char text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; + +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_ID_254_LEN 51 + #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 +#define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ + "STATUSTEXT", \ + 2, \ + { { "severity", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ + { "text", MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + } \ +} + + /** * @brief Pack a statustext message * @param system_id ID of this system @@ -24,19 +32,19 @@ 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 char* 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) { - mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - 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 + put_uint8_t_by_index(msg, 0, severity); // Severity of status, 0 = info message, 255 = critical fault + put_char_array_by_index(msg, 1, text, 50); // Status text message, without null termination character - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 51, 83); } /** - * @brief Pack a statustext message + * @brief Pack a statustext message on a channel * @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 @@ -45,16 +53,40 @@ 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 char* 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) +{ + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; + + put_uint8_t_by_index(msg, 0, severity); // Severity of status, 0 = info message, 255 = critical fault + put_char_array_by_index(msg, 1, text, 50); // Status text message, without null termination character + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51, 83); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a statustext message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param severity Severity of status, 0 = info message, 255 = critical fault + * @param text Status text message, without null termination character + */ +static inline void mavlink_msg_statustext_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t severity,const char *text) { - mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - 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 + put_uint8_t_by_index(msg, 0, severity); // Severity of status, 0 = info message, 255 = critical fault + put_char_array_by_index(msg, 1, text, 50); // Status text message, without null termination character - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 51, 83); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a statustext struct into a message @@ -69,8 +101,6 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a statustext message * @param chan MAVLink channel to send the message @@ -78,36 +108,19 @@ 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 */ -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) { - mavlink_header_t hdr; - mavlink_statustext_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_STATUSTEXT_LEN ) - payload.severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(payload.text, text, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x90, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 51); + mavlink_msg_statustext_pack_chan_send(chan, msg, severity, text); } #endif + // MESSAGE STATUSTEXT UNPACKING + /** * @brief Get field severity from statustext message * @@ -115,8 +128,7 @@ 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) { - mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; - return (uint8_t)(p->severity); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -124,12 +136,9 @@ 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, char* text) +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(text, p->text, sizeof(p->text)); - return sizeof(p->text); + return MAVLINK_MSG_RETURN_char_array(msg, text, 50, 1); } /** @@ -140,5 +149,10 @@ 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) { - memcpy( statustext, msg->payload, sizeof(mavlink_statustext_t)); +#if MAVLINK_NEED_BYTE_SWAP + statustext->severity = mavlink_msg_statustext_get_severity(msg); + mavlink_msg_statustext_get_text(msg, statustext->text); +#else + memcpy(statustext, MAVLINK_PAYLOAD(msg), 51); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index 708eac76b498944aa079e0fe796bb614f23de0e5..74e6e02cd1ee3216c63b250cdb88db653d262221 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -1,28 +1,47 @@ // MESSAGE SYS_STATUS PACKING #define MAVLINK_MSG_ID_SYS_STATUS 1 -#define MAVLINK_MSG_ID_SYS_STATUS_LEN 23 -#define MAVLINK_MSG_1_LEN 23 -#define MAVLINK_MSG_ID_SYS_STATUS_KEY 0xDA -#define MAVLINK_MSG_1_KEY 0xDA -typedef struct __mavlink_sys_status_t +typedef struct __mavlink_sys_status_t { - uint16_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - uint16_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - uint16_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt) - uint16_t current_battery; ///< Battery current, in milliamperes (1 = 1 milliampere) - uint16_t watt; ///< Watts consumed from this battery since startup - uint16_t errors_uart; ///< Dropped packets on all links (packets that were corrupted on reception on the MAV) - uint16_t errors_i2c; ///< Dropped packets on all links (packets that were corrupted on reception) - uint16_t errors_spi; ///< Dropped packets on all links (packets that were corrupted on reception) - uint16_t errors_can; ///< Dropped packets on all links (packets that were corrupted on reception) - uint8_t battery_percent; ///< Remaining battery energy: (0%: 0, 100%: 255) - + uint16_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + uint16_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + uint16_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt) + uint16_t current_battery; ///< Battery current, in milliamperes (1 = 1 milliampere) + uint16_t watt; ///< Watts consumed from this battery since startup + uint16_t errors_uart; ///< Dropped packets on all links (packets that were corrupted on reception on the MAV) + uint16_t errors_i2c; ///< Dropped packets on all links (packets that were corrupted on reception) + uint16_t errors_spi; ///< Dropped packets on all links (packets that were corrupted on reception) + uint16_t errors_can; ///< Dropped packets on all links (packets that were corrupted on reception) + uint8_t battery_percent; ///< Remaining battery energy: (0%: 0, 100%: 255) } mavlink_sys_status_t; +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 23 +#define MAVLINK_MSG_ID_1_LEN 23 + + + +#define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ + "SYS_STATUS", \ + 12, \ + { { "onboard_control_sensors_present", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \ + { "onboard_control_sensors_enabled", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \ + { "onboard_control_sensors_health", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \ + { "load", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_sys_status_t, load) }, \ + { "voltage_battery", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_sys_status_t, voltage_battery) }, \ + { "current_battery", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_sys_status_t, current_battery) }, \ + { "watt", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_sys_status_t, watt) }, \ + { "errors_uart", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, errors_uart) }, \ + { "errors_i2c", MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_sys_status_t, errors_i2c) }, \ + { "errors_spi", MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, errors_spi) }, \ + { "errors_can", MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_can) }, \ + { "battery_percent", MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_sys_status_t, battery_percent) }, \ + } \ +} + + /** * @brief Pack a sys_status message * @param system_id ID of this system @@ -43,29 +62,29 @@ typedef struct __mavlink_sys_status_t * @param errors_can Dropped packets on all links (packets that were corrupted on reception) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can) +static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - p->onboard_control_sensors_present = onboard_control_sensors_present; // uint16_t:Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - p->onboard_control_sensors_enabled = onboard_control_sensors_enabled; // uint16_t:Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - p->onboard_control_sensors_health = onboard_control_sensors_health; // uint16_t:Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->voltage_battery = voltage_battery; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->current_battery = current_battery; // uint16_t:Battery current, in milliamperes (1 = 1 milliampere) - p->watt = watt; // uint16_t:Watts consumed from this battery since startup - p->battery_percent = battery_percent; // uint8_t:Remaining battery energy: (0%: 0, 100%: 255) - p->errors_uart = errors_uart; // uint16_t:Dropped packets on all links (packets that were corrupted on reception on the MAV) - p->errors_i2c = errors_i2c; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - p->errors_spi = errors_spi; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - p->errors_can = errors_can; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); + put_uint16_t_by_index(msg, 0, onboard_control_sensors_present); // Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 2, onboard_control_sensors_enabled); // Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 4, onboard_control_sensors_health); // Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 6, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + put_uint16_t_by_index(msg, 8, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) + put_uint16_t_by_index(msg, 10, current_battery); // Battery current, in milliamperes (1 = 1 milliampere) + put_uint16_t_by_index(msg, 12, watt); // Watts consumed from this battery since startup + put_uint16_t_by_index(msg, 14, errors_uart); // Dropped packets on all links (packets that were corrupted on reception on the MAV) + put_uint16_t_by_index(msg, 16, errors_i2c); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 18, errors_spi); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 20, errors_can); // Dropped packets on all links (packets that were corrupted on reception) + put_uint8_t_by_index(msg, 22, battery_percent); // Remaining battery energy: (0%: 0, 100%: 255) + + return mavlink_finalize_message(msg, system_id, component_id, 23, 114); } /** - * @brief Pack a sys_status message + * @brief Pack a sys_status message on a channel * @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 @@ -84,26 +103,70 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @param errors_can Dropped packets on all links (packets that were corrupted on reception) * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can) +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, + uint16_t onboard_control_sensors_present,uint16_t onboard_control_sensors_enabled,uint16_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,uint16_t current_battery,uint16_t watt,uint8_t battery_percent,uint16_t errors_uart,uint16_t errors_i2c,uint16_t errors_spi,uint16_t errors_can) +{ + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; + + put_uint16_t_by_index(msg, 0, onboard_control_sensors_present); // Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 2, onboard_control_sensors_enabled); // Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 4, onboard_control_sensors_health); // Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 6, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + put_uint16_t_by_index(msg, 8, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) + put_uint16_t_by_index(msg, 10, current_battery); // Battery current, in milliamperes (1 = 1 milliampere) + put_uint16_t_by_index(msg, 12, watt); // Watts consumed from this battery since startup + put_uint16_t_by_index(msg, 14, errors_uart); // Dropped packets on all links (packets that were corrupted on reception on the MAV) + put_uint16_t_by_index(msg, 16, errors_i2c); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 18, errors_spi); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 20, errors_can); // Dropped packets on all links (packets that were corrupted on reception) + put_uint8_t_by_index(msg, 22, battery_percent); // Remaining battery energy: (0%: 0, 100%: 255) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23, 114); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a sys_status message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) + * @param current_battery Battery current, in milliamperes (1 = 1 milliampere) + * @param watt Watts consumed from this battery since startup + * @param battery_percent Remaining battery energy: (0%: 0, 100%: 255) + * @param errors_uart Dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_i2c Dropped packets on all links (packets that were corrupted on reception) + * @param errors_spi Dropped packets on all links (packets that were corrupted on reception) + * @param errors_can Dropped packets on all links (packets that were corrupted on reception) + */ +static inline void mavlink_msg_sys_status_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t onboard_control_sensors_present,uint16_t onboard_control_sensors_enabled,uint16_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,uint16_t current_battery,uint16_t watt,uint8_t battery_percent,uint16_t errors_uart,uint16_t errors_i2c,uint16_t errors_spi,uint16_t errors_can) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - p->onboard_control_sensors_present = onboard_control_sensors_present; // uint16_t:Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - p->onboard_control_sensors_enabled = onboard_control_sensors_enabled; // uint16_t:Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - p->onboard_control_sensors_health = onboard_control_sensors_health; // uint16_t:Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->voltage_battery = voltage_battery; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->current_battery = current_battery; // uint16_t:Battery current, in milliamperes (1 = 1 milliampere) - p->watt = watt; // uint16_t:Watts consumed from this battery since startup - p->battery_percent = battery_percent; // uint8_t:Remaining battery energy: (0%: 0, 100%: 255) - p->errors_uart = errors_uart; // uint16_t:Dropped packets on all links (packets that were corrupted on reception on the MAV) - p->errors_i2c = errors_i2c; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - p->errors_spi = errors_spi; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - p->errors_can = errors_can; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); + put_uint16_t_by_index(msg, 0, onboard_control_sensors_present); // Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 2, onboard_control_sensors_enabled); // Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 4, onboard_control_sensors_health); // Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + put_uint16_t_by_index(msg, 6, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + put_uint16_t_by_index(msg, 8, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) + put_uint16_t_by_index(msg, 10, current_battery); // Battery current, in milliamperes (1 = 1 milliampere) + put_uint16_t_by_index(msg, 12, watt); // Watts consumed from this battery since startup + put_uint16_t_by_index(msg, 14, errors_uart); // Dropped packets on all links (packets that were corrupted on reception on the MAV) + put_uint16_t_by_index(msg, 16, errors_i2c); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 18, errors_spi); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 20, errors_can); // Dropped packets on all links (packets that were corrupted on reception) + put_uint8_t_by_index(msg, 22, battery_percent); // Remaining battery energy: (0%: 0, 100%: 255) + + mavlink_finalize_message_chan_send(msg, chan, 23, 114); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a sys_status struct into a message @@ -118,8 +181,6 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->watt, sys_status->battery_percent, sys_status->errors_uart, sys_status->errors_i2c, sys_status->errors_spi, sys_status->errors_can); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a sys_status message * @param chan MAVLink channel to send the message @@ -137,46 +198,19 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param errors_spi Dropped packets on all links (packets that were corrupted on reception) * @param errors_can Dropped packets on all links (packets that were corrupted on reception) */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can) { - mavlink_header_t hdr; - mavlink_sys_status_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYS_STATUS_LEN ) - payload.onboard_control_sensors_present = onboard_control_sensors_present; // uint16_t:Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - payload.onboard_control_sensors_enabled = onboard_control_sensors_enabled; // uint16_t:Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - payload.onboard_control_sensors_health = onboard_control_sensors_health; // uint16_t:Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - payload.load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - payload.voltage_battery = voltage_battery; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - payload.current_battery = current_battery; // uint16_t:Battery current, in milliamperes (1 = 1 milliampere) - payload.watt = watt; // uint16_t:Watts consumed from this battery since startup - payload.battery_percent = battery_percent; // uint8_t:Remaining battery energy: (0%: 0, 100%: 255) - payload.errors_uart = errors_uart; // uint16_t:Dropped packets on all links (packets that were corrupted on reception on the MAV) - payload.errors_i2c = errors_i2c; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - payload.errors_spi = errors_spi; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - payload.errors_can = errors_can; // uint16_t:Dropped packets on all links (packets that were corrupted on reception) - - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xDA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 23); + mavlink_msg_sys_status_pack_chan_send(chan, msg, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, watt, battery_percent, errors_uart, errors_i2c, errors_spi, errors_can); } #endif + // MESSAGE SYS_STATUS UNPACKING + /** * @brief Get field onboard_control_sensors_present from sys_status message * @@ -184,8 +218,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint16_t */ static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->onboard_control_sensors_present); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -195,8 +228,7 @@ static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_presen */ static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->onboard_control_sensors_enabled); + return MAVLINK_MSG_RETURN_uint16_t(msg, 2); } /** @@ -206,8 +238,7 @@ static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_enable */ static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->onboard_control_sensors_health); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -217,8 +248,7 @@ static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_health */ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->load); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -228,8 +258,7 @@ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->voltage_battery); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -239,8 +268,7 @@ static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_ */ static inline uint16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->current_battery); + return MAVLINK_MSG_RETURN_uint16_t(msg, 10); } /** @@ -250,8 +278,7 @@ static inline uint16_t mavlink_msg_sys_status_get_current_battery(const mavlink_ */ static inline uint16_t mavlink_msg_sys_status_get_watt(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->watt); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -261,8 +288,7 @@ static inline uint16_t mavlink_msg_sys_status_get_watt(const mavlink_message_t* */ static inline uint8_t mavlink_msg_sys_status_get_battery_percent(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint8_t)(p->battery_percent); + return MAVLINK_MSG_RETURN_uint8_t(msg, 22); } /** @@ -272,8 +298,7 @@ static inline uint8_t mavlink_msg_sys_status_get_battery_percent(const mavlink_m */ static inline uint16_t mavlink_msg_sys_status_get_errors_uart(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->errors_uart); + return MAVLINK_MSG_RETURN_uint16_t(msg, 14); } /** @@ -283,8 +308,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_uart(const mavlink_mess */ static inline uint16_t mavlink_msg_sys_status_get_errors_i2c(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->errors_i2c); + return MAVLINK_MSG_RETURN_uint16_t(msg, 16); } /** @@ -294,8 +318,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_i2c(const mavlink_messa */ static inline uint16_t mavlink_msg_sys_status_get_errors_spi(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->errors_spi); + return MAVLINK_MSG_RETURN_uint16_t(msg, 18); } /** @@ -305,8 +328,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_spi(const mavlink_messa */ static inline uint16_t mavlink_msg_sys_status_get_errors_can(const mavlink_message_t* msg) { - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; - return (uint16_t)(p->errors_can); + return MAVLINK_MSG_RETURN_uint16_t(msg, 20); } /** @@ -317,5 +339,20 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_can(const mavlink_messa */ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) { - memcpy( sys_status, msg->payload, sizeof(mavlink_sys_status_t)); +#if MAVLINK_NEED_BYTE_SWAP + sys_status->onboard_control_sensors_present = mavlink_msg_sys_status_get_onboard_control_sensors_present(msg); + sys_status->onboard_control_sensors_enabled = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(msg); + sys_status->onboard_control_sensors_health = mavlink_msg_sys_status_get_onboard_control_sensors_health(msg); + sys_status->load = mavlink_msg_sys_status_get_load(msg); + sys_status->voltage_battery = mavlink_msg_sys_status_get_voltage_battery(msg); + sys_status->current_battery = mavlink_msg_sys_status_get_current_battery(msg); + sys_status->watt = mavlink_msg_sys_status_get_watt(msg); + sys_status->errors_uart = mavlink_msg_sys_status_get_errors_uart(msg); + sys_status->errors_i2c = mavlink_msg_sys_status_get_errors_i2c(msg); + sys_status->errors_spi = mavlink_msg_sys_status_get_errors_spi(msg); + sys_status->errors_can = mavlink_msg_sys_status_get_errors_can(msg); + sys_status->battery_percent = mavlink_msg_sys_status_get_battery_percent(msg); +#else + memcpy(sys_status, MAVLINK_PAYLOAD(msg), 23); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index c976e38c235623ef06aa03d9938bacd299a17d37..3e00e85aa0699a41d16d6f3f0cc7b8cc5116c8aa 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -1,18 +1,27 @@ // MESSAGE SYSTEM_TIME PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME 2 -#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 12 -#define MAVLINK_MSG_2_LEN 12 -#define MAVLINK_MSG_ID_SYSTEM_TIME_KEY 0x16 -#define MAVLINK_MSG_2_KEY 0x16 -typedef struct __mavlink_system_time_t +typedef struct __mavlink_system_time_t { - uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. - uint32_t time_boot_ms; ///< Timestamp of the component clock since boot time in milliseconds. - + uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. + uint32_t time_boot_ms; ///< Timestamp of the component clock since boot time in milliseconds. } mavlink_system_time_t; +#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 12 +#define MAVLINK_MSG_ID_2_LEN 12 + + + +#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME { \ + "SYSTEM_TIME", \ + 2, \ + { { "time_usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_usec) }, \ + { "time_boot_ms", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_system_time_t, time_boot_ms) }, \ + } \ +} + + /** * @brief Pack a system_time message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_system_time_t * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint32_t time_boot_ms) +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, uint32_t time_boot_ms) { - mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - p->time_boot_ms = time_boot_ms; // uint32_t:Timestamp of the component clock since boot time in milliseconds. + put_uint64_t_by_index(msg, 0, time_usec); // Timestamp of the master clock in microseconds since UNIX epoch. + put_uint32_t_by_index(msg, 8, time_boot_ms); // Timestamp of the component clock since boot time in milliseconds. - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 12, 143); } /** - * @brief Pack a system_time message + * @brief Pack a system_time message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint32_t time_boot_ms) +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,uint32_t time_boot_ms) +{ + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + + put_uint64_t_by_index(msg, 0, time_usec); // Timestamp of the master clock in microseconds since UNIX epoch. + put_uint32_t_by_index(msg, 8, time_boot_ms); // Timestamp of the component clock since boot time in milliseconds. + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 143); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a system_time message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. + * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. + */ +static inline void mavlink_msg_system_time_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t time_boot_ms) { - mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - p->time_boot_ms = time_boot_ms; // uint32_t:Timestamp of the component clock since boot time in milliseconds. + put_uint64_t_by_index(msg, 0, time_usec); // Timestamp of the master clock in microseconds since UNIX epoch. + put_uint32_t_by_index(msg, 8, time_boot_ms); // Timestamp of the component clock since boot time in milliseconds. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); + mavlink_finalize_message_chan_send(msg, chan, 12, 143); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a system_time struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_usec, system_time->time_boot_ms); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a system_time message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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. * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t time_boot_ms) { - mavlink_header_t hdr; - mavlink_system_time_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN ) - payload.time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - payload.time_boot_ms = time_boot_ms; // uint32_t:Timestamp of the component clock since boot time in milliseconds. - - 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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x16, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 12); + mavlink_msg_system_time_pack_chan_send(chan, msg, time_usec, time_boot_ms); } #endif + // MESSAGE SYSTEM_TIME UNPACKING + /** * @brief Get field time_usec from system_time message * @@ -114,8 +128,7 @@ 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) { - mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; - return (uint64_t)(p->time_usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -125,8 +138,7 @@ static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_messa */ static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_message_t* msg) { - mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; - return (uint32_t)(p->time_boot_ms); + return MAVLINK_MSG_RETURN_uint32_t(msg, 8); } /** @@ -137,5 +149,10 @@ static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_me */ static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) { - memcpy( system_time, msg->payload, sizeof(mavlink_system_time_t)); +#if MAVLINK_NEED_BYTE_SWAP + system_time->time_usec = mavlink_msg_system_time_get_time_usec(msg); + system_time->time_boot_ms = mavlink_msg_system_time_get_time_boot_ms(msg); +#else + memcpy(system_time, MAVLINK_PAYLOAD(msg), 12); +#endif } 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 ff299a39aa752f9e5c3397091544d9b223096391..dd4d130161d615a7fc96081d3b6f3d4f9d35cbc0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -1,18 +1,27 @@ // MESSAGE SYSTEM_TIME_UTC PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 3 -#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 -#define MAVLINK_MSG_3_LEN 8 -#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_KEY 0x4C -#define MAVLINK_MSG_3_KEY 0x4C -typedef struct __mavlink_system_time_utc_t +typedef struct __mavlink_system_time_utc_t { - uint32_t utc_date; ///< GPS UTC date ddmmyy - uint32_t utc_time; ///< GPS UTC time hhmmss - + uint32_t utc_date; ///< GPS UTC date ddmmyy + uint32_t utc_time; ///< GPS UTC time hhmmss } mavlink_system_time_utc_t; +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 +#define MAVLINK_MSG_ID_3_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC { \ + "SYSTEM_TIME_UTC", \ + 2, \ + { { "utc_date", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_system_time_utc_t, utc_date) }, \ + { "utc_time", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_system_time_utc_t, utc_time) }, \ + } \ +} + + /** * @brief Pack a system_time_utc message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_system_time_utc_t * @param utc_time GPS UTC time hhmmss * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + put_uint32_t_by_index(msg, 0, utc_date); // GPS UTC date ddmmyy + put_uint32_t_by_index(msg, 4, utc_time); // GPS UTC time hhmmss - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 8, 191); } /** - * @brief Pack a system_time_utc message + * @brief Pack a system_time_utc message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 * @param utc_time GPS UTC time hhmmss * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + + put_uint32_t_by_index(msg, 0, utc_date); // GPS UTC date ddmmyy + put_uint32_t_by_index(msg, 4, utc_time); // GPS UTC time hhmmss + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 191); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a system_time_utc message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + */ +static inline void mavlink_msg_system_time_utc_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint32_t utc_date,uint32_t utc_time) { - mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + put_uint32_t_by_index(msg, 0, utc_date); // GPS UTC date ddmmyy + put_uint32_t_by_index(msg, 4, utc_time); // GPS UTC time hhmmss - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); + mavlink_finalize_message_chan_send(msg, chan, 8, 191); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a system_time_utc struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin return mavlink_msg_system_time_utc_pack(system_id, component_id, msg, system_time_utc->utc_date, system_time_utc->utc_time); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a system_time_utc message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { - mavlink_header_t hdr; - mavlink_system_time_utc_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN ) - payload.utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x4C, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_system_time_utc_pack_chan_send(chan, msg, utc_date, utc_time); } #endif + // MESSAGE SYSTEM_TIME_UTC UNPACKING + /** * @brief Get field utc_date from system_time_utc message * @@ -114,8 +128,7 @@ 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) { - mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; - return (uint32_t)(p->utc_date); + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; - return (uint32_t)(p->utc_time); + return MAVLINK_MSG_RETURN_uint32_t(msg, 4); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( system_time_utc, msg->payload, sizeof(mavlink_system_time_utc_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(system_time_utc, MAVLINK_PAYLOAD(msg), 8); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index cfb6bc0783f992d98db5b9040ba5b0f5ee750dbc..ce0a4f9398e546bbb0f571af37671d02548f6132 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -1,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_VFR_HUD_KEY 0xFB -#define MAVLINK_MSG_74_KEY 0xFB -typedef struct __mavlink_vfr_hud_t +typedef struct __mavlink_vfr_hud_t { - float airspeed; ///< Current airspeed in m/s - float groundspeed; ///< Current ground speed in m/s - 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 - + float airspeed; ///< Current airspeed in m/s + float groundspeed; ///< Current ground speed in m/s + 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; +#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 +#define MAVLINK_MSG_ID_74_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_VFR_HUD { \ + "VFR_HUD", \ + 6, \ + { { "airspeed", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_vfr_hud_t, airspeed) }, \ + { "groundspeed", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_vfr_hud_t, groundspeed) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vfr_hud_t, alt) }, \ + { "climb", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vfr_hud_t, climb) }, \ + { "heading", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_vfr_hud_t, heading) }, \ + { "throttle", MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_vfr_hud_t, throttle) }, \ + } \ +} + + /** * @brief Pack a vfr_hud message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_vfr_hud_t * @param climb Current climb rate in meters/second * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - 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 + put_float_by_index(msg, 0, airspeed); // Current airspeed in m/s + put_float_by_index(msg, 4, groundspeed); // Current ground speed in m/s + put_float_by_index(msg, 8, alt); // Current altitude (MSL), in meters + put_float_by_index(msg, 12, climb); // Current climb rate in meters/second + put_int16_t_by_index(msg, 16, heading); // Current heading in degrees, in compass units (0..360, 0=north) + put_uint16_t_by_index(msg, 18, throttle); // Current throttle setting in integer percent, 0 to 100 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 20); } /** - * @brief Pack a vfr_hud message + * @brief Pack a vfr_hud message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo * @param climb Current climb rate in meters/second * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; + + put_float_by_index(msg, 0, airspeed); // Current airspeed in m/s + put_float_by_index(msg, 4, groundspeed); // Current ground speed in m/s + put_float_by_index(msg, 8, alt); // Current altitude (MSL), in meters + put_float_by_index(msg, 12, climb); // Current climb rate in meters/second + put_int16_t_by_index(msg, 16, heading); // Current heading in degrees, in compass units (0..360, 0=north) + put_uint16_t_by_index(msg, 18, throttle); // Current throttle setting in integer percent, 0 to 100 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 20); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a vfr_hud message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + */ +static inline void mavlink_msg_vfr_hud_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - 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 + put_float_by_index(msg, 0, airspeed); // Current airspeed in m/s + put_float_by_index(msg, 4, groundspeed); // Current ground speed in m/s + put_float_by_index(msg, 8, alt); // Current altitude (MSL), in meters + put_float_by_index(msg, 12, climb); // Current climb rate in meters/second + put_int16_t_by_index(msg, 16, heading); // Current heading in degrees, in compass units (0..360, 0=north) + put_uint16_t_by_index(msg, 18, throttle); // Current throttle setting in integer percent, 0 to 100 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); + mavlink_finalize_message_chan_send(msg, chan, 20, 20); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a vfr_hud struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vfr_hud message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_vfr_hud_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VFR_HUD_LEN ) - payload.airspeed = airspeed; // float:Current airspeed in m/s - payload.groundspeed = groundspeed; // float:Current ground speed in m/s - payload.heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) - payload.throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 - payload.alt = alt; // float:Current altitude (MSL), in meters - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xFB, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_vfr_hud_pack_chan_send(chan, msg, airspeed, groundspeed, heading, throttle, alt, climb); } #endif + // MESSAGE VFR_HUD UNPACKING + /** * @brief Get field airspeed from vfr_hud message * @@ -142,8 +164,7 @@ 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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; - return (float)(p->airspeed); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; - return (float)(p->groundspeed); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; - return (int16_t)(p->heading); + return MAVLINK_MSG_RETURN_int16_t(msg, 16); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; - return (uint16_t)(p->throttle); + return MAVLINK_MSG_RETURN_uint16_t(msg, 18); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; - return (float)(p->alt); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; - return (float)(p->climb); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( vfr_hud, msg->payload, sizeof(mavlink_vfr_hud_t)); +#if MAVLINK_NEED_BYTE_SWAP + vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); + vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); + vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); + vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); + vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); + vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); +#else + memcpy(vfr_hud, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index 900f257e737d3b10bd0abade9867af50b6ec2793..b0d4933db1ef677080ec9b3c2661d0d46927d38e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -1,30 +1,51 @@ // MESSAGE WAYPOINT PACKING #define MAVLINK_MSG_ID_WAYPOINT 39 -#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 -#define MAVLINK_MSG_39_LEN 36 -#define MAVLINK_MSG_ID_WAYPOINT_KEY 0xC5 -#define MAVLINK_MSG_39_KEY 0xC5 -typedef struct __mavlink_waypoint_t +typedef struct __mavlink_waypoint_t { - 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. - float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - 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 - + 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. + float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + 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; +#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 +#define MAVLINK_MSG_ID_39_LEN 36 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT { \ + "WAYPOINT", \ + 14, \ + { { "param1", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_waypoint_t, param1) }, \ + { "param2", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_waypoint_t, param2) }, \ + { "param3", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_waypoint_t, param3) }, \ + { "param4", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_waypoint_t, param4) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_waypoint_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_waypoint_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_waypoint_t, z) }, \ + { "seq", MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_waypoint_t, seq) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_waypoint_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_waypoint_t, target_component) }, \ + { "frame", MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_waypoint_t, frame) }, \ + { "command", MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_waypoint_t, command) }, \ + { "current", MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_waypoint_t, current) }, \ + { "autocontinue", MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_waypoint_t, autocontinue) }, \ + } \ +} + + /** * @brief Pack a waypoint message * @param system_id ID of this system @@ -47,31 +68,31 @@ typedef struct __mavlink_waypoint_t * @param z PARAM7 / z position: global: altitude * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - 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); + put_float_by_index(msg, 0, param1); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + put_float_by_index(msg, 4, param2); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + put_float_by_index(msg, 8, 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. + put_float_by_index(msg, 12, param4); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + put_float_by_index(msg, 16, x); // PARAM5 / local: x position, global: latitude + put_float_by_index(msg, 20, y); // PARAM6 / y position: global: longitude + put_float_by_index(msg, 24, z); // PARAM7 / z position: global: altitude + put_uint16_t_by_index(msg, 28, seq); // Sequence + put_uint8_t_by_index(msg, 30, target_system); // System ID + put_uint8_t_by_index(msg, 31, target_component); // Component ID + put_uint8_t_by_index(msg, 32, frame); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + put_uint8_t_by_index(msg, 33, command); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + put_uint8_t_by_index(msg, 34, current); // false:0, true:1 + put_uint8_t_by_index(msg, 35, autocontinue); // autocontinue to next wp + + return mavlink_finalize_message(msg, system_id, component_id, 36, 205); } /** - * @brief Pack a waypoint message + * @brief Pack a waypoint message on a channel * @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 @@ -92,28 +113,76 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp * @param z PARAM7 / z position: global: altitude * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; + + put_float_by_index(msg, 0, param1); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + put_float_by_index(msg, 4, param2); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + put_float_by_index(msg, 8, 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. + put_float_by_index(msg, 12, param4); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + put_float_by_index(msg, 16, x); // PARAM5 / local: x position, global: latitude + put_float_by_index(msg, 20, y); // PARAM6 / y position: global: longitude + put_float_by_index(msg, 24, z); // PARAM7 / z position: global: altitude + put_uint16_t_by_index(msg, 28, seq); // Sequence + put_uint8_t_by_index(msg, 30, target_system); // System ID + put_uint8_t_by_index(msg, 31, target_component); // Component ID + put_uint8_t_by_index(msg, 32, frame); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + put_uint8_t_by_index(msg, 33, command); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + put_uint8_t_by_index(msg, 34, current); // false:0, true:1 + put_uint8_t_by_index(msg, 35, autocontinue); // autocontinue to next wp + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 205); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint message on a channel and send + * @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 seq Sequence + * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + * @param current false:0, true:1 + * @param autocontinue autocontinue to next wp + * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + * @param 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. + * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude + * @param z PARAM7 / z position: global: altitude + */ +static inline void mavlink_msg_waypoint_pack_chan_send(mavlink_channel_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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - 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); + put_float_by_index(msg, 0, param1); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + put_float_by_index(msg, 4, param2); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + put_float_by_index(msg, 8, 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. + put_float_by_index(msg, 12, param4); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + put_float_by_index(msg, 16, x); // PARAM5 / local: x position, global: latitude + put_float_by_index(msg, 20, y); // PARAM6 / y position: global: longitude + put_float_by_index(msg, 24, z); // PARAM7 / z position: global: altitude + put_uint16_t_by_index(msg, 28, seq); // Sequence + put_uint8_t_by_index(msg, 30, target_system); // System ID + put_uint8_t_by_index(msg, 31, target_component); // Component ID + put_uint8_t_by_index(msg, 32, frame); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + put_uint8_t_by_index(msg, 33, command); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + put_uint8_t_by_index(msg, 34, current); // false:0, true:1 + put_uint8_t_by_index(msg, 35, autocontinue); // autocontinue to next wp + + mavlink_finalize_message_chan_send(msg, chan, 36, 205); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a waypoint struct into a message @@ -128,8 +197,6 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co return mavlink_msg_waypoint_pack(system_id, component_id, msg, waypoint->target_system, waypoint->target_component, waypoint->seq, waypoint->frame, waypoint->command, waypoint->current, waypoint->autocontinue, waypoint->param1, waypoint->param2, waypoint->param3, waypoint->param4, waypoint->x, waypoint->y, waypoint->z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint message * @param chan MAVLink channel to send the message @@ -149,48 +216,19 @@ 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 + 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_header_t hdr; - mavlink_waypoint_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.seq = seq; // uint16_t:Sequence - payload.frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - payload.command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - payload.current = current; // uint8_t:false:0, true:1 - payload.autocontinue = autocontinue; // uint8_t:autocontinue to next wp - payload.param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - payload.param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - payload.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. - payload.param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - payload.x = x; // float:PARAM5 / local: x position, global: latitude - payload.y = y; // float:PARAM6 / y position: global: longitude - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xC5, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 36); + mavlink_msg_waypoint_pack_chan_send(chan, msg, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); } #endif + // MESSAGE WAYPOINT UNPACKING + /** * @brief Get field target_system from waypoint message * @@ -198,8 +236,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 30); } /** @@ -209,8 +246,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 31); } /** @@ -220,8 +256,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint16_t)(p->seq); + return MAVLINK_MSG_RETURN_uint16_t(msg, 28); } /** @@ -231,8 +266,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint8_t)(p->frame); + return MAVLINK_MSG_RETURN_uint8_t(msg, 32); } /** @@ -242,8 +276,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint8_t)(p->command); + return MAVLINK_MSG_RETURN_uint8_t(msg, 33); } /** @@ -253,8 +286,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint8_t)(p->current); + return MAVLINK_MSG_RETURN_uint8_t(msg, 34); } /** @@ -264,8 +296,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (uint8_t)(p->autocontinue); + return MAVLINK_MSG_RETURN_uint8_t(msg, 35); } /** @@ -275,8 +306,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->param1); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -286,8 +316,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->param2); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -297,8 +326,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->param3); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -308,8 +336,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->param4); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -319,8 +346,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -330,8 +356,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -341,8 +366,7 @@ 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) { - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -353,5 +377,22 @@ 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) { - memcpy( waypoint, msg->payload, sizeof(mavlink_waypoint_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + waypoint->seq = mavlink_msg_waypoint_get_seq(msg); + waypoint->target_system = mavlink_msg_waypoint_get_target_system(msg); + waypoint->target_component = mavlink_msg_waypoint_get_target_component(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); +#else + memcpy(waypoint, MAVLINK_PAYLOAD(msg), 36); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 49e514987d76df7f35c76d4587a9ea5c6ad2b4fa..8fd8ad08653ccc9b5ebc8ea52cc24906ad204cac 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_ACK_KEY 0x9E -#define MAVLINK_MSG_47_KEY 0x9E -typedef struct __mavlink_waypoint_ack_t +typedef struct __mavlink_waypoint_ack_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t type; ///< 0: OK, 1: Error - + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t type; ///< 0: OK, 1: Error } mavlink_waypoint_ack_t; +#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 +#define MAVLINK_MSG_ID_47_LEN 3 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_ACK { \ + "WAYPOINT_ACK", \ + 3, \ + { { "target_system", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_ack_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_ack_t, target_component) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_ack_t, type) }, \ + } \ +} + + /** * @brief Pack a waypoint_ack message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_waypoint_ack_t * @param type 0: OK, 1: Error * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - 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 + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + put_uint8_t_by_index(msg, 2, type); // 0: OK, 1: Error - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 3, 214); } /** - * @brief Pack a waypoint_ack message + * @brief Pack a waypoint_ack message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t * @param type 0: OK, 1: Error * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - 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 + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + put_uint8_t_by_index(msg, 2, type); // 0: OK, 1: Error - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 214); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_ack message on a channel and send + * @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 type 0: OK, 1: Error + */ +static inline void mavlink_msg_waypoint_ack_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t type) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + put_uint8_t_by_index(msg, 2, type); // 0: OK, 1: Error + + mavlink_finalize_message_chan_send(msg, chan, 3, 214); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a waypoint_ack struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ return mavlink_msg_waypoint_ack_pack(system_id, component_id, msg, waypoint_ack->target_system, waypoint_ack->target_component, waypoint_ack->type); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_ack message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { - mavlink_header_t hdr; - mavlink_waypoint_ack_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x9E, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 3); + mavlink_msg_waypoint_ack_pack_chan_send(chan, msg, target_system, target_component, type); } #endif + // MESSAGE WAYPOINT_ACK UNPACKING + /** * @brief Get field target_system from waypoint_ack message * @@ -121,8 +137,7 @@ 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) { - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( waypoint_ack, msg->payload, sizeof(mavlink_waypoint_ack_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(waypoint_ack, MAVLINK_PAYLOAD(msg), 3); +#endif } 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 8a3c2b133fbd1213d80757d446002b58cb94a300..984f9fece2d8b0eb575ac0f99b1cd0aae95a013e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -1,18 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_KEY 0x22 -#define MAVLINK_MSG_45_KEY 0x22 -typedef struct __mavlink_waypoint_clear_all_t +typedef struct __mavlink_waypoint_clear_all_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_clear_all_t; +#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_ID_45_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL { \ + "WAYPOINT_CLEAR_ALL", \ + 2, \ + { { "target_system", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_clear_all_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_clear_all_t, target_component) }, \ + } \ +} + + /** * @brief Pack a waypoint_clear_all message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_waypoint_clear_all_t * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 229); } /** - * @brief Pack a waypoint_clear_all message + * @brief Pack a waypoint_clear_all message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 229); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_clear_all message on a channel and send + * @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 + */ +static inline void mavlink_msg_waypoint_clear_all_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) { - mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 229); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a waypoint_clear_all struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, return mavlink_msg_waypoint_clear_all_pack(system_id, component_id, msg, waypoint_clear_all->target_system, waypoint_clear_all->target_component); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_clear_all message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_header_t hdr; - mavlink_waypoint_clear_all_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x22, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_waypoint_clear_all_pack_chan_send(chan, msg, target_system, target_component); } #endif + // MESSAGE WAYPOINT_CLEAR_ALL UNPACKING + /** * @brief Get field target_system from waypoint_clear_all message * @@ -114,8 +128,7 @@ 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) { - mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( waypoint_clear_all, msg->payload, sizeof(mavlink_waypoint_clear_all_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(waypoint_clear_all, MAVLINK_PAYLOAD(msg), 2); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 5330bd2ae55244c23acc4b1f30d92c976b2557a9..7d652996f77c22458e8ab9119403d15fff0d4772 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_COUNT_KEY 0xE9 -#define MAVLINK_MSG_44_KEY 0xE9 -typedef struct __mavlink_waypoint_count_t +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 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_count_t; +#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 +#define MAVLINK_MSG_ID_44_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT { \ + "WAYPOINT_COUNT", \ + 3, \ + { { "count", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_count_t, count) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_count_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_count_t, target_component) }, \ + } \ +} + + /** * @brief Pack a waypoint_count message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_waypoint_count_t * @param count Number of Waypoints in the Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - 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 + put_uint16_t_by_index(msg, 0, count); // Number of Waypoints in the Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 8); } /** - * @brief Pack a waypoint_count message + * @brief Pack a waypoint_count message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ * @param count Number of Waypoints in the Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - 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 + put_uint16_t_by_index(msg, 0, count); // Number of Waypoints in the Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 8); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_count message on a channel and send + * @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 count Number of Waypoints in the Sequence + */ +static inline void mavlink_msg_waypoint_count_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t count) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + + put_uint16_t_by_index(msg, 0, count); // Number of Waypoints in the Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 4, 8); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a waypoint_count struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint return mavlink_msg_waypoint_count_pack(system_id, component_id, msg, waypoint_count->target_system, waypoint_count->target_component, waypoint_count->count); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_count message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { - mavlink_header_t hdr; - mavlink_waypoint_count_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xE9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_waypoint_count_pack_chan_send(chan, msg, target_system, target_component, count); } #endif + // MESSAGE WAYPOINT_COUNT UNPACKING + /** * @brief Get field target_system from waypoint_count message * @@ -121,8 +137,7 @@ 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) { - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; - return (uint16_t)(p->count); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( waypoint_count, msg->payload, sizeof(mavlink_waypoint_count_t)); +#if MAVLINK_NEED_BYTE_SWAP + waypoint_count->count = mavlink_msg_waypoint_count_get_count(msg); + waypoint_count->target_system = mavlink_msg_waypoint_count_get_target_system(msg); + waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); +#else + memcpy(waypoint_count, MAVLINK_PAYLOAD(msg), 4); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index b2f41ecd2ffa217d25cb65d7cb134fec008af055..8f1ebf1e321e59a7cfe1ae11cdfefe70d448e040 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -1,17 +1,25 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_KEY 0xA6 -#define MAVLINK_MSG_42_KEY 0xA6 -typedef struct __mavlink_waypoint_current_t +typedef struct __mavlink_waypoint_current_t { - uint16_t seq; ///< Sequence - + uint16_t seq; ///< Sequence } mavlink_waypoint_current_t; +#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 +#define MAVLINK_MSG_ID_42_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT { \ + "WAYPOINT_CURRENT", \ + 1, \ + { { "seq", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_current_t, seq) }, \ + } \ +} + + /** * @brief Pack a waypoint_current message * @param system_id ID of this system @@ -21,18 +29,18 @@ typedef struct __mavlink_waypoint_current_t * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seq) +static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seq) { - mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 101); } /** - * @brief Pack a waypoint_current message + * @brief Pack a waypoint_current message on a channel * @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 @@ -40,16 +48,38 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 101); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_current message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param seq Sequence + */ +static inline void mavlink_msg_waypoint_current_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t seq) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + + put_uint16_t_by_index(msg, 0, seq); // Sequence + + mavlink_finalize_message_chan_send(msg, chan, 2, 101); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a waypoint_current struct into a message * @@ -63,43 +93,25 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui return mavlink_msg_waypoint_current_pack(system_id, component_id, msg, waypoint_current->seq); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_current message * @param chan MAVLink channel to send the message * * @param seq Sequence */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { - mavlink_header_t hdr; - mavlink_waypoint_current_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN ) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA6, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_waypoint_current_pack_chan_send(chan, msg, seq); } #endif + // MESSAGE WAYPOINT_CURRENT UNPACKING + /** * @brief Get field seq from waypoint_current message * @@ -107,8 +119,7 @@ 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) { - mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; - return (uint16_t)(p->seq); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -119,5 +130,9 @@ 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) { - memcpy( waypoint_current, msg->payload, sizeof(mavlink_waypoint_current_t)); +#if MAVLINK_NEED_BYTE_SWAP + waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); +#else + memcpy(waypoint_current, MAVLINK_PAYLOAD(msg), 2); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 91d68ac6b3654e8d3bfdecf64f318e1842a6da03..665eacde65bdfb13e9f9e9137b5eee9c7d3419fe 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -1,17 +1,25 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_REACHED_KEY 0xA6 -#define MAVLINK_MSG_46_KEY 0xA6 -typedef struct __mavlink_waypoint_reached_t +typedef struct __mavlink_waypoint_reached_t { - uint16_t seq; ///< Sequence - + uint16_t seq; ///< Sequence } mavlink_waypoint_reached_t; +#define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 +#define MAVLINK_MSG_ID_46_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED { \ + "WAYPOINT_REACHED", \ + 1, \ + { { "seq", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_reached_t, seq) }, \ + } \ +} + + /** * @brief Pack a waypoint_reached message * @param system_id ID of this system @@ -21,18 +29,18 @@ typedef struct __mavlink_waypoint_reached_t * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seq) +static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seq) { - mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 21); } /** - * @brief Pack a waypoint_reached message + * @brief Pack a waypoint_reached message on a channel * @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 @@ -40,16 +48,38 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 21); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_reached message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param seq Sequence + */ +static inline void mavlink_msg_waypoint_reached_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t seq) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + + put_uint16_t_by_index(msg, 0, seq); // Sequence + + mavlink_finalize_message_chan_send(msg, chan, 2, 21); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a waypoint_reached struct into a message * @@ -63,43 +93,25 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui return mavlink_msg_waypoint_reached_pack(system_id, component_id, msg, waypoint_reached->seq); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_reached message * @param chan MAVLink channel to send the message * * @param seq Sequence */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { - mavlink_header_t hdr; - mavlink_waypoint_reached_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN ) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA6, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_waypoint_reached_pack_chan_send(chan, msg, seq); } #endif + // MESSAGE WAYPOINT_REACHED UNPACKING + /** * @brief Get field seq from waypoint_reached message * @@ -107,8 +119,7 @@ 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) { - mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; - return (uint16_t)(p->seq); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -119,5 +130,9 @@ 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) { - memcpy( waypoint_reached, msg->payload, sizeof(mavlink_waypoint_reached_t)); +#if MAVLINK_NEED_BYTE_SWAP + waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); +#else + memcpy(waypoint_reached, MAVLINK_PAYLOAD(msg), 2); +#endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index e32bf99e7fb3ea5bb8c5ee474243444523c9d76d..b50eeccf90904c5f43c5fb53e76586b7d66aedb1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_KEY 0xC0 -#define MAVLINK_MSG_40_KEY 0xC0 -typedef struct __mavlink_waypoint_request_t +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 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_request_t; +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 +#define MAVLINK_MSG_ID_40_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST { \ + "WAYPOINT_REQUEST", \ + 3, \ + { { "seq", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_request_t, seq) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_request_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_request_t, target_component) }, \ + } \ +} + + /** * @brief Pack a waypoint_request message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_waypoint_request_t * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 51); } /** - * @brief Pack a waypoint_request message + * @brief Pack a waypoint_request message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 51); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_request message on a channel and send + * @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 seq Sequence + */ +static inline void mavlink_msg_waypoint_request_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + + put_uint16_t_by_index(msg, 0, seq); // Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 4, 51); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a waypoint_request struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui return mavlink_msg_waypoint_request_pack(system_id, component_id, msg, waypoint_request->target_system, waypoint_request->target_component, waypoint_request->seq); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_request message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - mavlink_header_t hdr; - mavlink_waypoint_request_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xC0, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_waypoint_request_pack_chan_send(chan, msg, target_system, target_component, seq); } #endif + // MESSAGE WAYPOINT_REQUEST UNPACKING + /** * @brief Get field target_system from waypoint_request message * @@ -121,8 +137,7 @@ 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) { - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; - return (uint16_t)(p->seq); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( waypoint_request, msg->payload, sizeof(mavlink_waypoint_request_t)); +#if MAVLINK_NEED_BYTE_SWAP + waypoint_request->seq = mavlink_msg_waypoint_request_get_seq(msg); + waypoint_request->target_system = mavlink_msg_waypoint_request_get_target_system(msg); + waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); +#else + memcpy(waypoint_request, MAVLINK_PAYLOAD(msg), 4); +#endif } 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 8927107cc158bb7ab30424a9d042dc415c366d63..7160bd0605ed69d18916c298367417d352b13e82 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -1,18 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_KEY 0x22 -#define MAVLINK_MSG_43_KEY 0x22 -typedef struct __mavlink_waypoint_request_list_t +typedef struct __mavlink_waypoint_request_list_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_request_list_t; +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_43_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST { \ + "WAYPOINT_REQUEST_LIST", \ + 2, \ + { { "target_system", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_request_list_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_request_list_t, target_component) }, \ + } \ +} + + /** * @brief Pack a waypoint_request_list message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_waypoint_request_list_t * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 2, 213); } /** - * @brief Pack a waypoint_request_list message + * @brief Pack a waypoint_request_list message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 213); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_request_list message on a channel and send + * @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 + */ +static inline void mavlink_msg_waypoint_request_list_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) { - mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); + mavlink_finalize_message_chan_send(msg, chan, 2, 213); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a waypoint_request_list struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i return mavlink_msg_waypoint_request_list_pack(system_id, component_id, msg, waypoint_request_list->target_system, waypoint_request_list->target_component); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_request_list message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_header_t hdr; - mavlink_waypoint_request_list_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x22, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 2); + mavlink_msg_waypoint_request_list_pack_chan_send(chan, msg, target_system, target_component); } #endif + // MESSAGE WAYPOINT_REQUEST_LIST UNPACKING + /** * @brief Get field target_system from waypoint_request_list message * @@ -114,8 +128,7 @@ 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) { - mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( waypoint_request_list, msg->payload, sizeof(mavlink_waypoint_request_list_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(waypoint_request_list, MAVLINK_PAYLOAD(msg), 2); +#endif } 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 4b92c42319c659c60da7c2fe2b807e421d8d9f85..8bd54abb39172e31dae2a0bace8a9ca681b2fa5b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_KEY 0xC0 -#define MAVLINK_MSG_41_KEY 0xC0 -typedef struct __mavlink_waypoint_set_current_t +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 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_set_current_t; +#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 +#define MAVLINK_MSG_ID_41_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT { \ + "WAYPOINT_SET_CURRENT", \ + 3, \ + { { "seq", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_set_current_t, seq) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_set_current_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_set_current_t, target_component) }, \ + } \ +} + + /** * @brief Pack a waypoint_set_current message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_waypoint_set_current_t * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 80); } /** - * @brief Pack a waypoint_set_current message + * @brief Pack a waypoint_set_current message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + put_uint16_t_by_index(msg, 0, seq); // Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 80); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a waypoint_set_current message on a channel and send + * @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 seq Sequence + */ +static inline void mavlink_msg_waypoint_set_current_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t seq) +{ + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + + put_uint16_t_by_index(msg, 0, seq); // Sequence + put_uint8_t_by_index(msg, 2, target_system); // System ID + put_uint8_t_by_index(msg, 3, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 4, 80); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a waypoint_set_current struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id return mavlink_msg_waypoint_set_current_pack(system_id, component_id, msg, waypoint_set_current->target_system, waypoint_set_current->target_component, waypoint_set_current->seq); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_set_current message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + 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_header_t hdr; - mavlink_waypoint_set_current_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xC0, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_waypoint_set_current_pack_chan_send(chan, msg, target_system, target_component, seq); } #endif + // MESSAGE WAYPOINT_SET_CURRENT UNPACKING + /** * @brief Get field target_system from waypoint_set_current message * @@ -121,8 +137,7 @@ 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) { - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; - return (uint16_t)(p->seq); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( waypoint_set_current, msg->payload, sizeof(mavlink_waypoint_set_current_t)); +#if MAVLINK_NEED_BYTE_SWAP + waypoint_set_current->seq = mavlink_msg_waypoint_set_current_get_seq(msg); + 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); +#else + memcpy(waypoint_set_current, MAVLINK_PAYLOAD(msg), 4); +#endif } diff --git a/thirdParty/mavlink/include/common/testsuite.h b/thirdParty/mavlink/include/common/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..119d6af360a2097a8ff5a1d6b6f3782b6bbe1bbd --- /dev/null +++ b/thirdParty/mavlink/include/common/testsuite.h @@ -0,0 +1,2112 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from common.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef COMMON_TESTSUITE_H +#define COMMON_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_common(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + + mavlink_test_common(system_id, component_id); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_heartbeat_t packet2, packet1 = { + 5, + 72, + 139, + 206, + 17, + 84, + 151, + 3, + }; + if (sizeof(packet2) != 8) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.system_mode , packet1.flight_mode , packet1.system_status , packet1.safety_status , packet1.link_status ); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.system_mode , packet1.flight_mode , packet1.system_status , packet1.safety_status , packet1.link_status ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; imagic = MAVLINK_STX; + 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*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + + return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + */ +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint16_t length, uint8_t crc_extra) +{ + return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +MAVLINK_HELPER void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); + +/** + * @brief Finalize a MAVLink message with channel assignment and send + */ +MAVLINK_HELPER void mavlink_finalize_message_chan_send(mavlink_message_t* msg, + mavlink_channel_t chan, uint16_t length, uint8_t crc_extra) +{ + mavlink_finalize_message_chan(msg, mavlink_system.sysid, mavlink_system.compid, chan, length, crc_extra); + mavlink_send_uart(chan, msg); +} +#endif + +/** + * @brief Pack a message to send it over a serial byte stream + */ +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) +{ + memcpy(buffer, (uint8_t *)&msg->magic, msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES); + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->checksum); +} + +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->checksum); +} + +/** + * 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 + */ +MAVLINK_HELPER 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]; + + /* + default message crc function. You can override this per-system to + put this data in a different memory segment + */ +#if MAVLINK_CRC_EXTRA +#ifndef MAVLINK_MESSAGE_CRC + static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] +#endif +#endif + + 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; + rxmsg->len = 0; + 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.u8[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 MAVLINK_CRC_EXTRA + mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); +#endif + if (c != (rxmsg->checksum & 0xFF)) { + // 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; + rxmsg->len = 0; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != (rxmsg->checksum >> 8)) { + // 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; + rxmsg->len = 0; + 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)); + } + 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; + return status->msg_received; +} + +/** + * @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 + */ +MAVLINK_HELPER 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 + int32_t v; + uint8_t i_bit_index, i_byte_index, curr_bits_n; +#if MAVLINK_NEED_BYTE_SWAP + union { + int32_t i; + uint8_t b[4]; + } bin, bout; + 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]; + v = bout.i; +#else + v = b; +#endif + + // 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 <= (uint8_t)(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) & v); + + // 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; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define comm_send_ch() if you wish +// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a +// whole packet at a time + +/* + +#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); + } +} + */ + +MAVLINK_HELPER void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) +{ +#ifdef MAVLINK_SEND_UART_BYTES + /* this is the more efficient approach, if the platform + defines it */ + MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)&msg->magic, msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES); +#else + /* fallback to one byte at a time */ + uint8_t *buffer = (uint8_t *)&msg->magic; + uint16_t i; + for (i = 0; i < MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; i++) { + comm_send_ch(chan, buffer[i]); + } +#endif +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#endif /* _MAVLINK_HELPERS_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index e6526816a0196c837fb4d6be267217156163f8d8..7b9533064670a440148e50109a65f4431ceb42c8 100644 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -1,9 +1,3 @@ -/** @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_ @@ -52,98 +46,120 @@ enum MAV_ACTION 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_START_HILSIM = 41, + MAV_ACTION_STOP_HILSIM = 42, MAV_ACTION_NB ///< Number of MAV actions }; - - -#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_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< 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 -{ +typedef struct param_union { + union { + float param_float; + int32_t param_int32; + uint32_t param_uint32; + }; + uint8_t type; +} mavlink_param_union_t; + +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 + 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 word - struct - { - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; - }; - uint8_t STX; ///< start of packet marker +typedef struct __mavlink_message { + uint16_t checksum; /// sent at end of packet + uint8_t magic; ///< protocol magic 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 + union { ///< Payload data, ALIGNMENT IMPORTANT ON MCU + char c[MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES]; + int8_t i8[MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES]; + uint8_t u8[MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES]; + int16_t i16[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+1)/2]; + uint16_t u16[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+1)/2]; + int32_t i32[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+3)/4]; + uint32_t u32[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+3)/4]; + int64_t i64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; + uint64_t u64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; + float f[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+3)/4]; + double d[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; + } payload; } mavlink_message_t; -typedef struct __mavlink_header -{ - union - { - uint16_t ck; ///< Checksum word - 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 -} mavlink_header_t; +typedef enum { + MAVLINK_TYPE_CHAR = 0, + MAVLINK_TYPE_UINT8_T = 1, + MAVLINK_TYPE_INT8_T = 2, + MAVLINK_TYPE_UINT16_T = 3, + MAVLINK_TYPE_INT16_T = 4, + MAVLINK_TYPE_UINT32_T = 5, + MAVLINK_TYPE_INT32_T = 6, + MAVLINK_TYPE_UINT64_T = 7, + MAVLINK_TYPE_INT64_T = 8, + MAVLINK_TYPE_FLOAT = 9, + MAVLINK_TYPE_DOUBLE = 10 +} mavlink_message_type_t; -typedef enum -{ +#define MAVLINK_MAX_FIELDS 64 + +typedef struct __mavlink_field_info { + const char *name; // name of this field + mavlink_message_type_t type; // type of this field + unsigned array_length; // if non-zero, field is an array + unsigned wire_offset; // offset of each field in the payload + unsigned structure_offset; // offset in a C structure +} mavlink_field_info_t; + +// note that in this structure the order of fields is the order +// in the XML file, not necessary the wire order +typedef struct __mavlink_message_info { + const char *name; // name of the message + unsigned num_fields; // how many fields in this message + const mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information +} mavlink_message_info_t; + +#define MAVLINK_PAYLOAD(msg) msg->payload.u8 + +// checksum is immediately after the payload bytes +#define mavlink_ck_a(msg) msg->payload.u8[(msg)->len] +#define mavlink_ck_b(msg) msg->payload.u8[1+(uint16_t)(msg)->len] + +typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_4, - MAVLINK_COMM_5, - MAVLINK_COMM_6, - MAVLINK_COMM_7, - MAVLINK_COMM_8, - MAVLINK_COMM_9, - MAVLINK_COMM_10, - MAVLINK_COMM_11, - MAVLINK_COMM_12, - MAVLINK_COMM_13, - MAVLINK_COMM_14, - MAVLINK_COMM_15, - MAVLINK_COMM_NB_HIGH = 16 + MAVLINK_COMM_3 } mavlink_channel_t; -typedef enum -{ +/* + * 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, @@ -156,17 +172,7 @@ typedef enum MAVLINK_PARSE_STATE_GOT_CRC1 } mavlink_parse_state_t; ///< The state machine for the comm parser -typedef struct __mavlink_status -{ - union - { - uint16_t ck; ///< Checksum word - struct - { - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; - }; +typedef struct __mavlink_status { uint8_t msg_received; ///< Number of received messages uint8_t buffer_overrun; ///< Number of buffer overruns uint8_t parse_error; ///< Number of parse errors @@ -178,4 +184,7 @@ typedef struct __mavlink_status uint16_t packet_rx_drop_count; ///< Number of packet drops } mavlink_status_t; +#define MAVLINK_BIG_ENDIAN 0 +#define MAVLINK_LITTLE_ENDIAN 1 + #endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h.orig b/thirdParty/mavlink/include/mavlink_types.h.orig new file mode 100644 index 0000000000000000000000000000000000000000..69ed32ad059f67384dd922a18d741c58337c2fd4 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_types.h.orig @@ -0,0 +1,135 @@ +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +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 +}; + +#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 + 1) ///< 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_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length + +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 { + uint16_t checksum; /// sent at end of packet + uint8_t magic; ///< protocol magic 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+MAVLINK_NUM_CHECKSUM_BYTES]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +// checksum is immediately after the payload bytes +#define mavlink_ck_a(msg) msg->payload[msg->len] +#define mavlink_ck_b(msg) msg->payload[msg->len+1] + +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 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; + +#define MAVLINK_BIG_ENDIAN 0 +#define MAVLINK_LITTLE_ENDIAN 1 + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 7b1170e908ff5b994ec23fd22adcd56db84a8870..3244881eaf826e97b80f1d12af5301e75f5b9fde 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,16 +1,23 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol built from minimal.xml * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, August 20 2011, 11:07 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" +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif -#pragma pack(pop) + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 #endif + +#include "version.h" +#include "minimal.h" + +#endif // MAVLINK_H diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 5d1ca23a0c31e29e1a55f3dbbdfa0e28f2be293d..dfee24700552941ed648298c489472ed14fe4571 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -1,24 +1,39 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 -#define MAVLINK_MSG_ID_HEARTBEAT_LEN 8 -#define MAVLINK_MSG_0_LEN 8 -#define MAVLINK_MSG_ID_HEARTBEAT_KEY 0xB3 -#define MAVLINK_MSG_0_KEY 0xB3 -typedef struct __mavlink_heartbeat_t +typedef struct __mavlink_heartbeat_t { - uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - uint8_t autopilot; ///< Autopilot type / class. defined in MAV_CLASS ENUM - 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 - uint8_t safety_status; ///< System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - uint8_t link_status; ///< Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - uint8_t mavlink_version; ///< MAVLink version - + uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + uint8_t autopilot; ///< Autopilot type / class. defined in MAV_CLASS ENUM + 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 + uint8_t safety_status; ///< System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + uint8_t link_status; ///< Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + uint8_t mavlink_version; ///< MAVLink version } mavlink_heartbeat_t; +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 8 +#define MAVLINK_MSG_ID_0_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ + "HEARTBEAT", \ + 8, \ + { { "type", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_heartbeat_t, type) }, \ + { "autopilot", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_heartbeat_t, autopilot) }, \ + { "mode", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_heartbeat_t, mode) }, \ + { "nav_mode", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_heartbeat_t, nav_mode) }, \ + { "status", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, status) }, \ + { "safety_status", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, safety_status) }, \ + { "link_status", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, link_status) }, \ + { "mavlink_version", MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ + } \ +} + + /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -34,25 +49,25 @@ typedef struct __mavlink_heartbeat_t * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint8_t mode, uint8_t nav_mode, uint8_t status, uint8_t safety_status, uint8_t link_status) +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, uint8_t mode, uint8_t nav_mode, uint8_t status, uint8_t safety_status, uint8_t link_status) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - 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:Autopilot type / class. defined in MAV_CLASS ENUM - 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->safety_status = safety_status; // uint8_t:System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - p->link_status = link_status; // uint8_t:Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM + put_uint8_t_by_index(msg, 2, mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, nav_mode); // Navigation mode, see MAV_NAV_MODE ENUM + put_uint8_t_by_index(msg, 4, status); // System status flag, see MAV_STATUS ENUM + put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 7, 2); // MAVLink version - 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); + return mavlink_finalize_message(msg, system_id, component_id, 8, 11); } /** - * @brief Pack a heartbeat message + * @brief Pack a heartbeat message on a channel * @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 @@ -66,23 +81,58 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN * @return length of the message in bytes (excluding serial stream start sign) */ -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, uint8_t mode, uint8_t nav_mode, uint8_t status, uint8_t safety_status, uint8_t link_status) +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,uint8_t mode,uint8_t nav_mode,uint8_t status,uint8_t safety_status,uint8_t link_status) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - 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:Autopilot type / class. defined in MAV_CLASS ENUM - 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->safety_status = safety_status; // uint8_t:System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - p->link_status = link_status; // uint8_t:Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM + put_uint8_t_by_index(msg, 2, mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, nav_mode); // Navigation mode, see MAV_NAV_MODE ENUM + put_uint8_t_by_index(msg, 4, status); // System status flag, see MAV_STATUS ENUM + put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 7, 2); // MAVLink version - 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); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 11); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a heartbeat message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_CLASS ENUM + * @param mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM + * @param status System status flag, see MAV_STATUS ENUM + * @param safety_status System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + */ +static inline void mavlink_msg_heartbeat_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t autopilot,uint8_t mode,uint8_t nav_mode,uint8_t status,uint8_t safety_status,uint8_t link_status) +{ + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + + put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM + put_uint8_t_by_index(msg, 2, mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, nav_mode); // Navigation mode, see MAV_NAV_MODE ENUM + put_uint8_t_by_index(msg, 4, status); // System status flag, see MAV_STATUS ENUM + put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN + put_uint8_t_by_index(msg, 7, 2); // MAVLink version + + mavlink_finalize_message_chan_send(msg, chan, 8, 11); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a heartbeat struct into a message * @@ -96,8 +146,6 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->mode, heartbeat->nav_mode, heartbeat->status, heartbeat->safety_status, heartbeat->link_status); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -110,42 +158,19 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param safety_status System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation * @param link_status Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t mode, uint8_t nav_mode, uint8_t status, uint8_t safety_status, uint8_t link_status) { - mavlink_header_t hdr; - mavlink_heartbeat_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HEARTBEAT_LEN ) - payload.type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - payload.autopilot = autopilot; // uint8_t:Autopilot type / class. defined in MAV_CLASS ENUM - payload.mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - payload.nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - payload.status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - payload.safety_status = safety_status; // uint8_t:System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - payload.link_status = link_status; // uint8_t:Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xB3, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_heartbeat_pack_chan_send(chan, msg, type, autopilot, mode, nav_mode, status, safety_status, link_status); } #endif + // MESSAGE HEARTBEAT UNPACKING + /** * @brief Get field type from heartbeat message * @@ -153,8 +178,7 @@ 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) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -164,8 +188,7 @@ 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) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->autopilot); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -175,8 +198,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mode(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -186,8 +208,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_mode(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_nav_mode(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->nav_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -197,8 +218,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_nav_mode(const mavlink_message_t */ static inline uint8_t mavlink_msg_heartbeat_get_status(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->status); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -208,8 +228,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_status(const mavlink_message_t* */ static inline uint8_t mavlink_msg_heartbeat_get_safety_status(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->safety_status); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -219,8 +238,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_safety_status(const mavlink_mess */ static inline uint8_t mavlink_msg_heartbeat_get_link_status(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->link_status); + return MAVLINK_MSG_RETURN_uint8_t(msg, 6); } /** @@ -230,8 +248,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_link_status(const mavlink_messag */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; - return (uint8_t)(p->mavlink_version); + return MAVLINK_MSG_RETURN_uint8_t(msg, 7); } /** @@ -242,5 +259,16 @@ 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) { - memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); +#if MAVLINK_NEED_BYTE_SWAP + heartbeat->type = mavlink_msg_heartbeat_get_type(msg); + heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); + heartbeat->mode = mavlink_msg_heartbeat_get_mode(msg); + heartbeat->nav_mode = mavlink_msg_heartbeat_get_nav_mode(msg); + heartbeat->status = mavlink_msg_heartbeat_get_status(msg); + heartbeat->safety_status = mavlink_msg_heartbeat_get_safety_status(msg); + heartbeat->link_status = mavlink_msg_heartbeat_get_link_status(msg); + heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); +#else + memcpy(heartbeat, MAVLINK_PAYLOAD(msg), 8); +#endif } diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index f4a8c6774dad0f0b497a5fbf60cf1d058613629b..4b0e7919c6015696cf9ee14c517ac8893595b95c 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from minimal.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 20 2011, 11:07 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -10,11 +9,26 @@ extern "C" { #endif +// MESSAGE LENGTHS AND CRCS -#include "../mavlink_protocol.h" +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}} +#endif + +#include "../protocol.h" #define MAVLINK_ENABLED_MINIMAL + + // MAVLINK VERSION #ifndef MAVLINK_VERSION @@ -29,22 +43,11 @@ extern "C" { // ENUM DEFINITIONS -// MESSAGE DEFINITIONS +// MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" - -// MESSAGE CRC KEYS - -#undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { } - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { } - #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // MINIMAL_H diff --git a/thirdParty/mavlink/include/minimal/testsuite.h b/thirdParty/mavlink/include/minimal/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..cbbff8d58b26424dde6eb9276bf01a66d87373ff --- /dev/null +++ b/thirdParty/mavlink/include/minimal/testsuite.h @@ -0,0 +1,66 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from minimal.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef MINIMAL_TESTSUITE_H +#define MINIMAL_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_minimal(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + + mavlink_test_minimal(system_id, component_id); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_heartbeat_t packet2, packet1 = { + 5, + 72, + 139, + 206, + 17, + 84, + 151, + 2, + }; + if (sizeof(packet2) != 8) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.mode , packet1.nav_mode , packet1.status , packet1.safety_status , packet1.link_status ); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.mode , packet1.nav_mode , packet1.status , packet1.safety_status , packet1.link_status ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ipayload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - 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, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); + put_float_by_index(msg, 0, roll); // roll + put_float_by_index(msg, 4, pitch); // pitch + put_float_by_index(msg, 8, yaw); // yaw + put_float_by_index(msg, 12, thrust); // thrust + put_uint8_t_by_index(msg, 16, target); // The system to be controlled + put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 + put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 + put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 + put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + + return mavlink_finalize_message(msg, system_id, component_id, 21, 254); } /** - * @brief Pack a attitude_control message + * @brief Pack a attitude_control message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint * @param thrust_manual thrust auto:0, manual:1 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - 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, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); + put_float_by_index(msg, 0, roll); // roll + put_float_by_index(msg, 4, pitch); // pitch + put_float_by_index(msg, 8, yaw); // yaw + put_float_by_index(msg, 12, thrust); // thrust + put_uint8_t_by_index(msg, 16, target); // The system to be controlled + put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 + put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 + put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 + put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 254); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a attitude_control message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system to be controlled + * @param roll roll + * @param pitch pitch + * @param yaw yaw + * @param thrust thrust + * @param roll_manual roll control enabled auto:0, manual:1 + * @param pitch_manual pitch auto:0, manual:1 + * @param yaw_manual yaw auto:0, manual:1 + * @param thrust_manual thrust auto:0, manual:1 + */ +static inline void mavlink_msg_attitude_control_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + + put_float_by_index(msg, 0, roll); // roll + put_float_by_index(msg, 4, pitch); // pitch + put_float_by_index(msg, 8, yaw); // yaw + put_float_by_index(msg, 12, thrust); // thrust + put_uint8_t_by_index(msg, 16, target); // The system to be controlled + put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 + put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 + put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 + put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + + mavlink_finalize_message_chan_send(msg, chan, 21, 254); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a attitude_control struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui return mavlink_msg_attitude_control_pack(system_id, component_id, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a attitude_control message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_attitude_control_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN ) - payload.target = target; // uint8_t:The system to be controlled - payload.roll = roll; // float:roll - payload.pitch = pitch; // float:pitch - payload.yaw = yaw; // float:yaw - payload.thrust = thrust; // float:thrust - payload.roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - payload.pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - payload.yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x7F, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 21); + mavlink_msg_attitude_control_pack_chan_send(chan, msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); } #endif + // MESSAGE ATTITUDE_CONTROL UNPACKING + /** * @brief Get field target from attitude_control message * @@ -163,8 +191,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (float)(p->thrust); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (uint8_t)(p->roll_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (uint8_t)(p->pitch_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 18); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (uint8_t)(p->yaw_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 19); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; - return (uint8_t)(p->thrust_manual); + return MAVLINK_MSG_RETURN_uint8_t(msg, 20); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( attitude_control, msg->payload, sizeof(mavlink_attitude_control_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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->target = mavlink_msg_attitude_control_get_target(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); +#else + memcpy(attitude_control, MAVLINK_PAYLOAD(msg), 21); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index a82b77d5323c3459d3bc8d07f04eec1a5121b01e..7f444d0afca094f0f39be7a832592334f0481f00 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -1,25 +1,39 @@ // 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 -#define MAVLINK_MSG_ID_BRIEF_FEATURE_KEY 0xD9 -#define MAVLINK_MSG_172_KEY 0xD9 +#define MAVLINK_MSG_ID_BRIEF_FEATURE 195 -typedef struct __mavlink_brief_feature_t +typedef struct __mavlink_brief_feature_t { - float x; ///< x position in m - float y; ///< y position in m - float z; ///< z position in m - 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 x; ///< x position in m + float y; ///< y position in m + float z; ///< z position in m + 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 } mavlink_brief_feature_t; + +#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 +#define MAVLINK_MSG_ID_195_LEN 53 + #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 +#define MAVLINK_MESSAGE_INFO_BRIEF_FEATURE { \ + "BRIEF_FEATURE", \ + 8, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_brief_feature_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_brief_feature_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_brief_feature_t, z) }, \ + { "response", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_brief_feature_t, response) }, \ + { "size", MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_brief_feature_t, size) }, \ + { "orientation", MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_brief_feature_t, orientation) }, \ + { "orientation_assignment", MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_brief_feature_t, orientation_assignment) }, \ + { "descriptor", MAVLINK_TYPE_UINT8_T, 32, 21, offsetof(mavlink_brief_feature_t, descriptor) }, \ + } \ +} + + /** * @brief Pack a brief_feature message * @param system_id ID of this system @@ -36,25 +50,25 @@ typedef struct __mavlink_brief_feature_t * @param response Harris operator response at this location * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - 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 + put_float_by_index(msg, 0, x); // x position in m + put_float_by_index(msg, 4, y); // y position in m + put_float_by_index(msg, 8, z); // z position in m + put_float_by_index(msg, 12, response); // Harris operator response at this location + put_uint16_t_by_index(msg, 16, size); // Size in pixels + put_uint16_t_by_index(msg, 18, orientation); // Orientation + put_uint8_t_by_index(msg, 20, orientation_assignment); // Orientation assignment 0: false, 1:true + put_uint8_t_array_by_index(msg, 21, descriptor, 32); // Descriptor - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 53, 88); } /** - * @brief Pack a brief_feature message + * @brief Pack a brief_feature message on a channel * @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 @@ -69,22 +83,58 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t * @param response Harris operator response at this location * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + + put_float_by_index(msg, 0, x); // x position in m + put_float_by_index(msg, 4, y); // y position in m + put_float_by_index(msg, 8, z); // z position in m + put_float_by_index(msg, 12, response); // Harris operator response at this location + put_uint16_t_by_index(msg, 16, size); // Size in pixels + put_uint16_t_by_index(msg, 18, orientation); // Orientation + put_uint8_t_by_index(msg, 20, orientation_assignment); // Orientation assignment 0: false, 1:true + put_uint8_t_array_by_index(msg, 21, descriptor, 32); // Descriptor + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 53, 88); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a brief_feature message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param x x position in m + * @param y y position in m + * @param z z position in m + * @param orientation_assignment Orientation assignment 0: false, 1:true + * @param size Size in pixels + * @param orientation Orientation + * @param descriptor Descriptor + * @param response Harris operator response at this location + */ +static inline void mavlink_msg_brief_feature_pack_chan_send(mavlink_channel_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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - 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 + put_float_by_index(msg, 0, x); // x position in m + put_float_by_index(msg, 4, y); // y position in m + put_float_by_index(msg, 8, z); // z position in m + put_float_by_index(msg, 12, response); // Harris operator response at this location + put_uint16_t_by_index(msg, 16, size); // Size in pixels + put_uint16_t_by_index(msg, 18, orientation); // Orientation + put_uint8_t_by_index(msg, 20, orientation_assignment); // Orientation assignment 0: false, 1:true + put_uint8_t_array_by_index(msg, 21, descriptor, 32); // Descriptor - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 53, 88); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a brief_feature struct into a message @@ -99,8 +149,6 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a brief_feature message * @param chan MAVLink channel to send the message @@ -114,42 +162,19 @@ 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 */ -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) +#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_header_t hdr; - mavlink_brief_feature_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN ) - payload.x = x; // float:x position in m - payload.y = y; // float:y position in m - payload.z = z; // float:z position in m - payload.orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true - payload.size = size; // uint16_t:Size in pixels - payload.orientation = orientation; // uint16_t:Orientation - memcpy(payload.descriptor, descriptor, sizeof(payload.descriptor)); // uint8_t[32]:Descriptor - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xD9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 53); + mavlink_msg_brief_feature_pack_chan_send(chan, msg, x, y, z, orientation_assignment, size, orientation, descriptor, response); } #endif + // MESSAGE BRIEF_FEATURE UNPACKING + /** * @brief Get field x from brief_feature message * @@ -157,8 +182,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -168,8 +192,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -179,8 +202,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -190,8 +212,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (uint8_t)(p->orientation_assignment); + return MAVLINK_MSG_RETURN_uint8_t(msg, 20); } /** @@ -201,8 +222,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (uint16_t)(p->size); + return MAVLINK_MSG_RETURN_uint16_t(msg, 16); } /** @@ -212,8 +232,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (uint16_t)(p->orientation); + return MAVLINK_MSG_RETURN_uint16_t(msg, 18); } /** @@ -221,12 +240,9 @@ 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* descriptor) +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(descriptor, p->descriptor, sizeof(p->descriptor)); - return sizeof(p->descriptor); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, descriptor, 32, 21); } /** @@ -236,8 +252,7 @@ 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) { - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - return (float)(p->response); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -248,5 +263,16 @@ 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) { - memcpy( brief_feature, msg->payload, sizeof(mavlink_brief_feature_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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->response = mavlink_msg_brief_feature_get_response(msg); + brief_feature->size = mavlink_msg_brief_feature_get_size(msg); + brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); + brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); + mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); +#else + memcpy(brief_feature, MAVLINK_PAYLOAD(msg), 53); +#endif } 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 1d471de9e552b08e0216ab73b131e7659c6fd3e9..acd946ca6a76c450a6ef1dd68221c06d78c86bba 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -1,21 +1,33 @@ // 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 -#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_KEY 0xC8 -#define MAVLINK_MSG_170_KEY 0xC8 +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 193 -typedef struct __mavlink_data_transmission_handshake_t +typedef struct __mavlink_data_transmission_handshake_t { - 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] - + 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; +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 8 +#define MAVLINK_MSG_ID_193_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE { \ + "DATA_TRANSMISSION_HANDSHAKE", \ + 5, \ + { { "size", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, type) }, \ + { "packets", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_data_transmission_handshake_t, packets) }, \ + { "payload", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, payload) }, \ + { "jpg_quality", MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \ + } \ +} + + /** * @brief Pack a data_transmission_handshake message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_data_transmission_handshake_t * @param jpg_quality JPEG quality out of [1,100] * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - 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] + put_uint32_t_by_index(msg, 0, size); // total data size in bytes (set on ACK only) + put_uint8_t_by_index(msg, 4, type); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + put_uint8_t_by_index(msg, 5, packets); // number of packets beeing sent (set on ACK only) + put_uint8_t_by_index(msg, 6, payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + put_uint8_t_by_index(msg, 7, jpg_quality); // JPEG quality out of [1,100] - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 8, 148); } /** - * @brief Pack a data_transmission_handshake message + * @brief Pack a data_transmission_handshake message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst * @param jpg_quality JPEG quality out of [1,100] * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - 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] + put_uint32_t_by_index(msg, 0, size); // total data size in bytes (set on ACK only) + put_uint8_t_by_index(msg, 4, type); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + put_uint8_t_by_index(msg, 5, packets); // number of packets beeing sent (set on ACK only) + put_uint8_t_by_index(msg, 6, payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + put_uint8_t_by_index(msg, 7, jpg_quality); // JPEG quality out of [1,100] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 148); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a data_transmission_handshake message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + * @param size total data size in bytes (set on ACK only) + * @param packets number of packets beeing sent (set on ACK only) + * @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] + */ +static inline void mavlink_msg_data_transmission_handshake_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t type,uint32_t size,uint8_t packets,uint8_t payload,uint8_t jpg_quality) +{ + msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; + + put_uint32_t_by_index(msg, 0, size); // total data size in bytes (set on ACK only) + put_uint8_t_by_index(msg, 4, type); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + put_uint8_t_by_index(msg, 5, packets); // number of packets beeing sent (set on ACK only) + put_uint8_t_by_index(msg, 6, payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + put_uint8_t_by_index(msg, 7, jpg_quality); // JPEG quality out of [1,100] + + mavlink_finalize_message_chan_send(msg, chan, 8, 148); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a data_transmission_handshake struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a data_transmission_handshake message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ 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 + 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_header_t hdr; - mavlink_data_transmission_handshake_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN ) - payload.type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - payload.size = size; // uint32_t:total data size in bytes (set on ACK only) - payload.packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) - payload.payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xC8, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 8); + mavlink_msg_data_transmission_handshake_pack_chan_send(chan, msg, type, size, packets, payload, jpg_quality); } #endif + // MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING + /** * @brief Get field type from data_transmission_handshake message * @@ -135,8 +155,7 @@ 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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -146,8 +165,7 @@ 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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; - return (uint32_t)(p->size); + return MAVLINK_MSG_RETURN_uint32_t(msg, 0); } /** @@ -157,8 +175,7 @@ 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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; - return (uint8_t)(p->packets); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -168,8 +185,7 @@ 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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; - return (uint8_t)(p->payload); + return MAVLINK_MSG_RETURN_uint8_t(msg, 6); } /** @@ -179,8 +195,7 @@ 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) { - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; - return (uint8_t)(p->jpg_quality); + return MAVLINK_MSG_RETURN_uint8_t(msg, 7); } /** @@ -191,5 +206,13 @@ 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) { - memcpy( data_transmission_handshake, msg->payload, sizeof(mavlink_data_transmission_handshake_t)); +#if MAVLINK_NEED_BYTE_SWAP + data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); + data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(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); +#else + memcpy(data_transmission_handshake, MAVLINK_PAYLOAD(msg), 8); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index 942a55d1e114a1e0428bd6da17decbb197e4723e..86f10d6cba9ca66ca2d4cec7128da2dac2229b98 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -1,19 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_KEY 0x87 -#define MAVLINK_MSG_171_KEY 0x87 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194 -typedef struct __mavlink_encapsulated_data_t +typedef struct __mavlink_encapsulated_data_t { - uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) - uint8_t data[253]; ///< image data bytes - + uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) + uint8_t data[253]; ///< image data bytes } mavlink_encapsulated_data_t; + +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 +#define MAVLINK_MSG_ID_194_LEN 255 + #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 +#define MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA { \ + "ENCAPSULATED_DATA", \ + 2, \ + { { "seqnr", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_encapsulated_data_t, seqnr) }, \ + { "data", MAVLINK_TYPE_UINT8_T, 253, 2, offsetof(mavlink_encapsulated_data_t, data) }, \ + } \ +} + + /** * @brief Pack a encapsulated_data message * @param system_id ID of this system @@ -24,19 +32,19 @@ typedef struct __mavlink_encapsulated_data_t * @param data image data bytes * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - 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 + put_uint16_t_by_index(msg, 0, seqnr); // sequence number (starting with 0 on every transmission) + put_uint8_t_array_by_index(msg, 2, data, 253); // image data bytes - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 255, 223); } /** - * @brief Pack a encapsulated_data message + * @brief Pack a encapsulated_data message on a channel * @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 @@ -45,16 +53,40 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin * @param data image data bytes * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; + + put_uint16_t_by_index(msg, 0, seqnr); // sequence number (starting with 0 on every transmission) + put_uint8_t_array_by_index(msg, 2, data, 253); // image data bytes + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 223); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a encapsulated_data message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param seqnr sequence number (starting with 0 on every transmission) + * @param data image data bytes + */ +static inline void mavlink_msg_encapsulated_data_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t seqnr,const uint8_t *data) { - mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - 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 + put_uint16_t_by_index(msg, 0, seqnr); // sequence number (starting with 0 on every transmission) + put_uint8_t_array_by_index(msg, 2, data, 253); // image data bytes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); + mavlink_finalize_message_chan_send(msg, chan, 255, 223); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a encapsulated_data struct into a message @@ -69,8 +101,6 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a encapsulated_data message * @param chan MAVLink channel to send the message @@ -78,36 +108,19 @@ 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 */ -static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) +#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_header_t hdr; - mavlink_encapsulated_data_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN ) - payload.seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) - memcpy(payload.data, data, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x87, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 255); + mavlink_msg_encapsulated_data_pack_chan_send(chan, msg, seqnr, data); } #endif + // MESSAGE ENCAPSULATED_DATA UNPACKING + /** * @brief Get field seqnr from encapsulated_data message * @@ -115,8 +128,7 @@ 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) { - mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; - return (uint16_t)(p->seqnr); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -124,12 +136,9 @@ 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* 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(data, p->data, sizeof(p->data)); - return sizeof(p->data); + return MAVLINK_MSG_RETURN_uint8_t_array(msg, data, 253, 2); } /** @@ -140,5 +149,10 @@ 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) { - memcpy( encapsulated_data, msg->payload, sizeof(mavlink_encapsulated_data_t)); +#if MAVLINK_NEED_BYTE_SWAP + encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); + mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); +#else + memcpy(encapsulated_data, MAVLINK_PAYLOAD(msg), 255); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index 30004ff4325ce8a142f0e2246e8e3593614ee278..fae307c4d386b6057723d28661b81d8bc65b3d6b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -1,39 +1,69 @@ // 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 -#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_KEY 0xA5 -#define MAVLINK_MSG_103_KEY 0xA5 +#define MAVLINK_MSG_ID_IMAGE_AVAILABLE 154 -typedef struct __mavlink_image_available_t +typedef struct __mavlink_image_available_t { - uint64_t cam_id; ///< Camera id - 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 - uint32_t key; ///< Shared memory area key - uint32_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - 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 - + uint64_t cam_id; ///< Camera id + 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 + uint32_t key; ///< Shared memory area key + uint32_t exposure; ///< Exposure time, in microseconds + float gain; ///< Camera gain + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad + float local_z; ///< Local frame Z coordinate (height over ground) + float lat; ///< GPS X coordinate + float lon; ///< GPS Y coordinate + float alt; ///< Global frame altitude + 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; +#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 +#define MAVLINK_MSG_ID_154_LEN 92 + + + +#define MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE { \ + "IMAGE_AVAILABLE", \ + 23, \ + { { "cam_id", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_available_t, cam_id) }, \ + { "timestamp", MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_image_available_t, timestamp) }, \ + { "valid_until", MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_image_available_t, valid_until) }, \ + { "img_seq", MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_image_available_t, img_seq) }, \ + { "img_buf_index", MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_image_available_t, img_buf_index) }, \ + { "key", MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_image_available_t, key) }, \ + { "exposure", MAVLINK_TYPE_UINT32_T, 0, 36, offsetof(mavlink_image_available_t, exposure) }, \ + { "gain", MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_available_t, gain) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_available_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_available_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_image_available_t, yaw) }, \ + { "local_z", MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_image_available_t, local_z) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_image_available_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_image_available_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_image_available_t, alt) }, \ + { "ground_x", MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_image_available_t, ground_x) }, \ + { "ground_y", MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_image_available_t, ground_y) }, \ + { "ground_z", MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_image_available_t, ground_z) }, \ + { "width", MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_image_available_t, width) }, \ + { "height", MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_image_available_t, height) }, \ + { "depth", MAVLINK_TYPE_UINT16_T, 0, 88, offsetof(mavlink_image_available_t, depth) }, \ + { "cam_no", MAVLINK_TYPE_UINT8_T, 0, 90, offsetof(mavlink_image_available_t, cam_no) }, \ + { "channels", MAVLINK_TYPE_UINT8_T, 0, 91, offsetof(mavlink_image_available_t, channels) }, \ + } \ +} + + /** * @brief Pack a image_available message * @param system_id ID of this system @@ -65,40 +95,40 @@ typedef struct __mavlink_image_available_t * @param ground_z Ground truth Z * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - 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); + put_uint64_t_by_index(msg, 0, cam_id); // Camera id + put_uint64_t_by_index(msg, 8, timestamp); // Timestamp + put_uint64_t_by_index(msg, 16, valid_until); // Until which timestamp this buffer will stay valid + put_uint32_t_by_index(msg, 24, img_seq); // The image sequence number + put_uint32_t_by_index(msg, 28, img_buf_index); // Position of the image in the buffer, starts with 0 + put_uint32_t_by_index(msg, 32, key); // Shared memory area key + put_uint32_t_by_index(msg, 36, exposure); // Exposure time, in microseconds + put_float_by_index(msg, 40, gain); // Camera gain + put_float_by_index(msg, 44, roll); // Roll angle in rad + put_float_by_index(msg, 48, pitch); // Pitch angle in rad + put_float_by_index(msg, 52, yaw); // Yaw angle in rad + put_float_by_index(msg, 56, local_z); // Local frame Z coordinate (height over ground) + put_float_by_index(msg, 60, lat); // GPS X coordinate + put_float_by_index(msg, 64, lon); // GPS Y coordinate + put_float_by_index(msg, 68, alt); // Global frame altitude + put_float_by_index(msg, 72, ground_x); // Ground truth X + put_float_by_index(msg, 76, ground_y); // Ground truth Y + put_float_by_index(msg, 80, ground_z); // Ground truth Z + put_uint16_t_by_index(msg, 84, width); // Image width + put_uint16_t_by_index(msg, 86, height); // Image height + put_uint16_t_by_index(msg, 88, depth); // Image depth + put_uint8_t_by_index(msg, 90, cam_no); // Camera # (starts with 0) + put_uint8_t_by_index(msg, 91, channels); // Image channels + + return mavlink_finalize_message(msg, system_id, component_id, 92, 224); } /** - * @brief Pack a image_available message + * @brief Pack a image_available message on a channel * @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 @@ -128,38 +158,104 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 * @param ground_z Ground truth Z * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - 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); + put_uint64_t_by_index(msg, 0, cam_id); // Camera id + put_uint64_t_by_index(msg, 8, timestamp); // Timestamp + put_uint64_t_by_index(msg, 16, valid_until); // Until which timestamp this buffer will stay valid + put_uint32_t_by_index(msg, 24, img_seq); // The image sequence number + put_uint32_t_by_index(msg, 28, img_buf_index); // Position of the image in the buffer, starts with 0 + put_uint32_t_by_index(msg, 32, key); // Shared memory area key + put_uint32_t_by_index(msg, 36, exposure); // Exposure time, in microseconds + put_float_by_index(msg, 40, gain); // Camera gain + put_float_by_index(msg, 44, roll); // Roll angle in rad + put_float_by_index(msg, 48, pitch); // Pitch angle in rad + put_float_by_index(msg, 52, yaw); // Yaw angle in rad + put_float_by_index(msg, 56, local_z); // Local frame Z coordinate (height over ground) + put_float_by_index(msg, 60, lat); // GPS X coordinate + put_float_by_index(msg, 64, lon); // GPS Y coordinate + put_float_by_index(msg, 68, alt); // Global frame altitude + put_float_by_index(msg, 72, ground_x); // Ground truth X + put_float_by_index(msg, 76, ground_y); // Ground truth Y + put_float_by_index(msg, 80, ground_z); // Ground truth Z + put_uint16_t_by_index(msg, 84, width); // Image width + put_uint16_t_by_index(msg, 86, height); // Image height + put_uint16_t_by_index(msg, 88, depth); // Image depth + put_uint8_t_by_index(msg, 90, cam_no); // Camera # (starts with 0) + put_uint8_t_by_index(msg, 91, channels); // Image channels + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 92, 224); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a image_available message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param cam_id Camera id + * @param cam_no Camera # (starts with 0) + * @param timestamp Timestamp + * @param valid_until Until which timestamp this buffer will stay valid + * @param img_seq The image sequence number + * @param img_buf_index Position of the image in the buffer, starts with 0 + * @param width Image width + * @param height Image height + * @param depth Image depth + * @param channels Image channels + * @param key Shared memory area key + * @param exposure Exposure time, in microseconds + * @param gain Camera gain + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @param local_z Local frame Z coordinate (height over ground) + * @param lat GPS X coordinate + * @param lon GPS Y coordinate + * @param alt Global frame altitude + * @param ground_x Ground truth X + * @param ground_y Ground truth Y + * @param ground_z Ground truth Z + */ +static inline void mavlink_msg_image_available_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; + + put_uint64_t_by_index(msg, 0, cam_id); // Camera id + put_uint64_t_by_index(msg, 8, timestamp); // Timestamp + put_uint64_t_by_index(msg, 16, valid_until); // Until which timestamp this buffer will stay valid + put_uint32_t_by_index(msg, 24, img_seq); // The image sequence number + put_uint32_t_by_index(msg, 28, img_buf_index); // Position of the image in the buffer, starts with 0 + put_uint32_t_by_index(msg, 32, key); // Shared memory area key + put_uint32_t_by_index(msg, 36, exposure); // Exposure time, in microseconds + put_float_by_index(msg, 40, gain); // Camera gain + put_float_by_index(msg, 44, roll); // Roll angle in rad + put_float_by_index(msg, 48, pitch); // Pitch angle in rad + put_float_by_index(msg, 52, yaw); // Yaw angle in rad + put_float_by_index(msg, 56, local_z); // Local frame Z coordinate (height over ground) + put_float_by_index(msg, 60, lat); // GPS X coordinate + put_float_by_index(msg, 64, lon); // GPS Y coordinate + put_float_by_index(msg, 68, alt); // Global frame altitude + put_float_by_index(msg, 72, ground_x); // Ground truth X + put_float_by_index(msg, 76, ground_y); // Ground truth Y + put_float_by_index(msg, 80, ground_z); // Ground truth Z + put_uint16_t_by_index(msg, 84, width); // Image width + put_uint16_t_by_index(msg, 86, height); // Image height + put_uint16_t_by_index(msg, 88, depth); // Image depth + put_uint8_t_by_index(msg, 90, cam_no); // Camera # (starts with 0) + put_uint8_t_by_index(msg, 91, channels); // Image channels + + mavlink_finalize_message_chan_send(msg, chan, 92, 224); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a image_available struct into a message * @@ -173,8 +269,6 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a image_available message * @param chan MAVLink channel to send the message @@ -203,57 +297,19 @@ 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 + 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_header_t hdr; - mavlink_image_available_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN ) - payload.cam_id = cam_id; // uint64_t:Camera id - payload.cam_no = cam_no; // uint8_t:Camera # (starts with 0) - payload.timestamp = timestamp; // uint64_t:Timestamp - payload.valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid - payload.img_seq = img_seq; // uint32_t:The image sequence number - payload.img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 - payload.width = width; // uint16_t:Image width - payload.height = height; // uint16_t:Image height - payload.depth = depth; // uint16_t:Image depth - payload.channels = channels; // uint8_t:Image channels - payload.key = key; // uint32_t:Shared memory area key - payload.exposure = exposure; // uint32_t:Exposure time, in microseconds - payload.gain = gain; // float:Camera gain - payload.roll = roll; // float:Roll angle in rad - payload.pitch = pitch; // float:Pitch angle in rad - payload.yaw = yaw; // float:Yaw angle in rad - payload.local_z = local_z; // float:Local frame Z coordinate (height over ground) - payload.lat = lat; // float:GPS X coordinate - payload.lon = lon; // float:GPS Y coordinate - payload.alt = alt; // float:Global frame altitude - payload.ground_x = ground_x; // float:Ground truth X - payload.ground_y = ground_y; // float:Ground truth Y - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA5, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 92); + mavlink_msg_image_available_pack_chan_send(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); } #endif + // MESSAGE IMAGE_AVAILABLE UNPACKING + /** * @brief Get field cam_id from image_available message * @@ -261,8 +317,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint64_t)(p->cam_id); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -272,8 +327,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint8_t)(p->cam_no); + return MAVLINK_MSG_RETURN_uint8_t(msg, 90); } /** @@ -283,8 +337,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint64_t)(p->timestamp); + return MAVLINK_MSG_RETURN_uint64_t(msg, 8); } /** @@ -294,8 +347,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint64_t)(p->valid_until); + return MAVLINK_MSG_RETURN_uint64_t(msg, 16); } /** @@ -305,8 +357,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint32_t)(p->img_seq); + return MAVLINK_MSG_RETURN_uint32_t(msg, 24); } /** @@ -316,8 +367,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint32_t)(p->img_buf_index); + return MAVLINK_MSG_RETURN_uint32_t(msg, 28); } /** @@ -327,8 +377,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint16_t)(p->width); + return MAVLINK_MSG_RETURN_uint16_t(msg, 84); } /** @@ -338,8 +387,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint16_t)(p->height); + return MAVLINK_MSG_RETURN_uint16_t(msg, 86); } /** @@ -349,8 +397,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint16_t)(p->depth); + return MAVLINK_MSG_RETURN_uint16_t(msg, 88); } /** @@ -360,8 +407,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint8_t)(p->channels); + return MAVLINK_MSG_RETURN_uint8_t(msg, 91); } /** @@ -371,8 +417,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint32_t)(p->key); + return MAVLINK_MSG_RETURN_uint32_t(msg, 32); } /** @@ -382,8 +427,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (uint32_t)(p->exposure); + return MAVLINK_MSG_RETURN_uint32_t(msg, 36); } /** @@ -393,8 +437,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->gain); + return MAVLINK_MSG_RETURN_float(msg, 40); } /** @@ -404,8 +447,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 44); } /** @@ -415,8 +457,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 48); } /** @@ -426,8 +467,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 52); } /** @@ -437,8 +477,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->local_z); + return MAVLINK_MSG_RETURN_float(msg, 56); } /** @@ -448,8 +487,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->lat); + return MAVLINK_MSG_RETURN_float(msg, 60); } /** @@ -459,8 +497,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->lon); + return MAVLINK_MSG_RETURN_float(msg, 64); } /** @@ -470,8 +507,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->alt); + return MAVLINK_MSG_RETURN_float(msg, 68); } /** @@ -481,8 +517,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->ground_x); + return MAVLINK_MSG_RETURN_float(msg, 72); } /** @@ -492,8 +527,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->ground_y); + return MAVLINK_MSG_RETURN_float(msg, 76); } /** @@ -503,8 +537,7 @@ 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) { - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; - return (float)(p->ground_z); + return MAVLINK_MSG_RETURN_float(msg, 80); } /** @@ -515,5 +548,31 @@ 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) { - memcpy( image_available, msg->payload, sizeof(mavlink_image_available_t)); +#if MAVLINK_NEED_BYTE_SWAP + image_available->cam_id = mavlink_msg_image_available_get_cam_id(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->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); + 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->cam_no = mavlink_msg_image_available_get_cam_no(msg); + image_available->channels = mavlink_msg_image_available_get_channels(msg); +#else + memcpy(image_available, MAVLINK_PAYLOAD(msg), 92); +#endif } 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 4396378ef38ac07c1febe37afef93b2ceeaebb99..48713c9e987d2bf84b0675727614e95d6b843625 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -1,17 +1,25 @@ // 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 -#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_KEY 0xEE -#define MAVLINK_MSG_102_KEY 0xEE +#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 153 -typedef struct __mavlink_image_trigger_control_t +typedef struct __mavlink_image_trigger_control_t { - uint8_t enable; ///< 0 to disable, 1 to enable - + uint8_t enable; ///< 0 to disable, 1 to enable } mavlink_image_trigger_control_t; +#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 +#define MAVLINK_MSG_ID_153_LEN 1 + + + +#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL { \ + "IMAGE_TRIGGER_CONTROL", \ + 1, \ + { { "enable", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_image_trigger_control_t, enable) }, \ + } \ +} + + /** * @brief Pack a image_trigger_control message * @param system_id ID of this system @@ -21,18 +29,18 @@ typedef struct __mavlink_image_trigger_control_t * @param enable 0 to disable, 1 to enable * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - p->enable = enable; // uint8_t:0 to disable, 1 to enable + put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 1, 95); } /** - * @brief Pack a image_trigger_control message + * @brief Pack a image_trigger_control message on a channel * @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 @@ -40,16 +48,38 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, * @param enable 0 to disable, 1 to enable * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - p->enable = enable; // uint8_t:0 to disable, 1 to enable + put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1, 95); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a image_trigger_control message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param enable 0 to disable, 1 to enable + */ +static inline void mavlink_msg_image_trigger_control_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t enable) +{ + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + + put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable + + mavlink_finalize_message_chan_send(msg, chan, 1, 95); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a image_trigger_control struct into a message * @@ -63,43 +93,25 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a image_trigger_control message * @param chan MAVLink channel to send the message * * @param enable 0 to disable, 1 to enable */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { - mavlink_header_t hdr; - mavlink_image_trigger_control_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN ) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xEE, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 1); + mavlink_msg_image_trigger_control_pack_chan_send(chan, msg, enable); } #endif + // MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING + /** * @brief Get field enable from image_trigger_control message * @@ -107,8 +119,7 @@ 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) { - mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; - return (uint8_t)(p->enable); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -119,5 +130,9 @@ 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) { - memcpy( image_trigger_control, msg->payload, sizeof(mavlink_image_trigger_control_t)); +#if MAVLINK_NEED_BYTE_SWAP + image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); +#else + memcpy(image_trigger_control, MAVLINK_PAYLOAD(msg), 1); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 5375ae3257e6c0ccdd7b96029359ab1c08d19ca3..a6c48878df009dbcbedd16d1ffd23c02df6674c9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -1,28 +1,47 @@ // 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 -#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_KEY 0x8 -#define MAVLINK_MSG_101_KEY 0x8 +#define MAVLINK_MSG_ID_IMAGE_TRIGGERED 152 -typedef struct __mavlink_image_triggered_t +typedef struct __mavlink_image_triggered_t { - uint64_t timestamp; ///< Timestamp - uint32_t seq; ///< IMU seq - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z - + uint64_t timestamp; ///< Timestamp + uint32_t seq; ///< IMU seq + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad + float local_z; ///< Local frame Z coordinate (height over ground) + float lat; ///< GPS X coordinate + float lon; ///< GPS Y coordinate + float alt; ///< Global frame altitude + float ground_x; ///< Ground truth X + float ground_y; ///< Ground truth Y + float ground_z; ///< Ground truth Z } mavlink_image_triggered_t; +#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 +#define MAVLINK_MSG_ID_152_LEN 52 + + + +#define MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED { \ + "IMAGE_TRIGGERED", \ + 12, \ + { { "timestamp", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_image_triggered_t, timestamp) }, \ + { "seq", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_image_triggered_t, seq) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_image_triggered_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_image_triggered_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_image_triggered_t, yaw) }, \ + { "local_z", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_image_triggered_t, local_z) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_image_triggered_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_image_triggered_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_image_triggered_t, alt) }, \ + { "ground_x", MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_image_triggered_t, ground_x) }, \ + { "ground_y", MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_image_triggered_t, ground_y) }, \ + { "ground_z", MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_image_triggered_t, ground_z) }, \ + } \ +} + + /** * @brief Pack a image_triggered message * @param system_id ID of this system @@ -43,29 +62,29 @@ typedef struct __mavlink_image_triggered_t * @param ground_z Ground truth Z * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - 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, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); + put_uint64_t_by_index(msg, 0, timestamp); // Timestamp + put_uint32_t_by_index(msg, 8, seq); // IMU seq + put_float_by_index(msg, 12, roll); // Roll angle in rad + put_float_by_index(msg, 16, pitch); // Pitch angle in rad + put_float_by_index(msg, 20, yaw); // Yaw angle in rad + put_float_by_index(msg, 24, local_z); // Local frame Z coordinate (height over ground) + put_float_by_index(msg, 28, lat); // GPS X coordinate + put_float_by_index(msg, 32, lon); // GPS Y coordinate + put_float_by_index(msg, 36, alt); // Global frame altitude + put_float_by_index(msg, 40, ground_x); // Ground truth X + put_float_by_index(msg, 44, ground_y); // Ground truth Y + put_float_by_index(msg, 48, ground_z); // Ground truth Z + + return mavlink_finalize_message(msg, system_id, component_id, 52, 86); } /** - * @brief Pack a image_triggered message + * @brief Pack a image_triggered message on a channel * @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 @@ -84,26 +103,70 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 * @param ground_z Ground truth Z * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; + + put_uint64_t_by_index(msg, 0, timestamp); // Timestamp + put_uint32_t_by_index(msg, 8, seq); // IMU seq + put_float_by_index(msg, 12, roll); // Roll angle in rad + put_float_by_index(msg, 16, pitch); // Pitch angle in rad + put_float_by_index(msg, 20, yaw); // Yaw angle in rad + put_float_by_index(msg, 24, local_z); // Local frame Z coordinate (height over ground) + put_float_by_index(msg, 28, lat); // GPS X coordinate + put_float_by_index(msg, 32, lon); // GPS Y coordinate + put_float_by_index(msg, 36, alt); // Global frame altitude + put_float_by_index(msg, 40, ground_x); // Ground truth X + put_float_by_index(msg, 44, ground_y); // Ground truth Y + put_float_by_index(msg, 48, ground_z); // Ground truth Z + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 52, 86); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a image_triggered message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param timestamp Timestamp + * @param seq IMU seq + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + * @param local_z Local frame Z coordinate (height over ground) + * @param lat GPS X coordinate + * @param lon GPS Y coordinate + * @param alt Global frame altitude + * @param ground_x Ground truth X + * @param ground_y Ground truth Y + * @param ground_z Ground truth Z + */ +static inline void mavlink_msg_image_triggered_pack_chan_send(mavlink_channel_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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - 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, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); + put_uint64_t_by_index(msg, 0, timestamp); // Timestamp + put_uint32_t_by_index(msg, 8, seq); // IMU seq + put_float_by_index(msg, 12, roll); // Roll angle in rad + put_float_by_index(msg, 16, pitch); // Pitch angle in rad + put_float_by_index(msg, 20, yaw); // Yaw angle in rad + put_float_by_index(msg, 24, local_z); // Local frame Z coordinate (height over ground) + put_float_by_index(msg, 28, lat); // GPS X coordinate + put_float_by_index(msg, 32, lon); // GPS Y coordinate + put_float_by_index(msg, 36, alt); // Global frame altitude + put_float_by_index(msg, 40, ground_x); // Ground truth X + put_float_by_index(msg, 44, ground_y); // Ground truth Y + put_float_by_index(msg, 48, ground_z); // Ground truth Z + + mavlink_finalize_message_chan_send(msg, chan, 52, 86); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a image_triggered struct into a message @@ -118,8 +181,6 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a image_triggered message * @param chan MAVLink channel to send the message @@ -137,46 +198,19 @@ 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 + 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_header_t hdr; - mavlink_image_triggered_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN ) - payload.timestamp = timestamp; // uint64_t:Timestamp - payload.seq = seq; // uint32_t:IMU seq - payload.roll = roll; // float:Roll angle in rad - payload.pitch = pitch; // float:Pitch angle in rad - payload.yaw = yaw; // float:Yaw angle in rad - payload.local_z = local_z; // float:Local frame Z coordinate (height over ground) - payload.lat = lat; // float:GPS X coordinate - payload.lon = lon; // float:GPS Y coordinate - payload.alt = alt; // float:Global frame altitude - payload.ground_x = ground_x; // float:Ground truth X - payload.ground_y = ground_y; // float:Ground truth Y - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x8, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 52); + mavlink_msg_image_triggered_pack_chan_send(chan, msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); } #endif + // MESSAGE IMAGE_TRIGGERED UNPACKING + /** * @brief Get field timestamp from image_triggered message * @@ -184,8 +218,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (uint64_t)(p->timestamp); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -195,8 +228,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (uint32_t)(p->seq); + return MAVLINK_MSG_RETURN_uint32_t(msg, 8); } /** @@ -206,8 +238,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -217,8 +248,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -228,8 +258,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -239,8 +268,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->local_z); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -250,8 +278,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->lat); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -261,8 +288,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->lon); + return MAVLINK_MSG_RETURN_float(msg, 32); } /** @@ -272,8 +298,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->alt); + return MAVLINK_MSG_RETURN_float(msg, 36); } /** @@ -283,8 +308,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->ground_x); + return MAVLINK_MSG_RETURN_float(msg, 40); } /** @@ -294,8 +318,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->ground_y); + return MAVLINK_MSG_RETURN_float(msg, 44); } /** @@ -305,8 +328,7 @@ 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) { - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; - return (float)(p->ground_z); + return MAVLINK_MSG_RETURN_float(msg, 48); } /** @@ -317,5 +339,20 @@ 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) { - memcpy( image_triggered, msg->payload, sizeof(mavlink_image_triggered_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(image_triggered, MAVLINK_PAYLOAD(msg), 52); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index eac24cd065b2adbf4a46c28e5718162d6ff70297..6a845909d1647c1a294f629d3479caade499653e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -1,23 +1,37 @@ // MESSAGE MARKER PACKING -#define MAVLINK_MSG_ID_MARKER 130 -#define MAVLINK_MSG_ID_MARKER_LEN 26 -#define MAVLINK_MSG_130_LEN 26 -#define MAVLINK_MSG_ID_MARKER_KEY 0xDD -#define MAVLINK_MSG_130_KEY 0xDD +#define MAVLINK_MSG_ID_MARKER 171 -typedef struct __mavlink_marker_t +typedef struct __mavlink_marker_t { - 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 - + 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; +#define MAVLINK_MSG_ID_MARKER_LEN 26 +#define MAVLINK_MSG_ID_171_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_MARKER { \ + "MARKER", \ + 7, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_marker_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_marker_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_marker_t, z) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_marker_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_marker_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_marker_t, yaw) }, \ + { "id", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_marker_t, id) }, \ + } \ +} + + /** * @brief Pack a marker message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_marker_t * @param yaw yaw orientation * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, roll); // roll orientation + put_float_by_index(msg, 16, pitch); // pitch orientation + put_float_by_index(msg, 20, yaw); // yaw orientation + put_uint16_t_by_index(msg, 24, id); // ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 26, 249); } /** - * @brief Pack a marker message + * @brief Pack a marker message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon * @param yaw yaw orientation * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, roll); // roll orientation + put_float_by_index(msg, 16, pitch); // pitch orientation + put_float_by_index(msg, 20, yaw); // yaw orientation + put_uint16_t_by_index(msg, 24, id); // ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 249); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a marker message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param id ID + * @param x x position + * @param y y position + * @param z z position + * @param roll roll orientation + * @param pitch pitch orientation + * @param yaw yaw orientation + */ +static inline void mavlink_msg_marker_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t id,float x,float y,float z,float roll,float pitch,float yaw) +{ + msg->msgid = MAVLINK_MSG_ID_MARKER; + + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, roll); // roll orientation + put_float_by_index(msg, 16, pitch); // pitch orientation + put_float_by_index(msg, 20, yaw); // yaw orientation + put_uint16_t_by_index(msg, 24, id); // ID + + mavlink_finalize_message_chan_send(msg, chan, 26, 249); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a marker struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp return mavlink_msg_marker_pack(system_id, component_id, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a marker message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_marker_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MARKER_LEN ) - payload.id = id; // uint16_t:ID - payload.x = x; // float:x position - payload.y = y; // float:y position - payload.z = z; // float:z position - payload.roll = roll; // float:roll orientation - payload.pitch = pitch; // float:pitch orientation - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xDD, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 26); + mavlink_msg_marker_pack_chan_send(chan, msg, id, x, y, z, roll, pitch, yaw); } #endif + // MESSAGE MARKER UNPACKING + /** * @brief Get field id from marker message * @@ -149,8 +173,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (uint16_t)(p->id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 24); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( marker, msg->payload, sizeof(mavlink_marker_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + marker->id = mavlink_msg_marker_get_id(msg); +#else + memcpy(marker, MAVLINK_PAYLOAD(msg), 26); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index f880295ccbda6ca0c92792c56afbde119a2ebb37..07d369f38ae14dcd6bf751d5aeac02049f65fdba 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -1,21 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_PATTERN_DETECTED_KEY 0x34 -#define MAVLINK_MSG_160_KEY 0x34 +#define MAVLINK_MSG_ID_PATTERN_DETECTED 190 -typedef struct __mavlink_pattern_detected_t +typedef struct __mavlink_pattern_detected_t { - float confidence; ///< Confidence of detection - uint8_t type; ///< 0: Pattern, 1: Letter - char file[100]; ///< Pattern file name - uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes - + float confidence; ///< Confidence of detection + 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_ID_PATTERN_DETECTED_LEN 106 +#define MAVLINK_MSG_ID_190_LEN 106 + #define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 +#define MAVLINK_MESSAGE_INFO_PATTERN_DETECTED { \ + "PATTERN_DETECTED", \ + 4, \ + { { "confidence", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pattern_detected_t, confidence) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_pattern_detected_t, type) }, \ + { "file", MAVLINK_TYPE_CHAR, 100, 5, offsetof(mavlink_pattern_detected_t, file) }, \ + { "detected", MAVLINK_TYPE_UINT8_T, 0, 105, offsetof(mavlink_pattern_detected_t, detected) }, \ + } \ +} + + /** * @brief Pack a pattern_detected message * @param system_id ID of this system @@ -28,21 +38,21 @@ 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 char* 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) { - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - 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 + put_float_by_index(msg, 0, confidence); // Confidence of detection + put_uint8_t_by_index(msg, 4, type); // 0: Pattern, 1: Letter + put_char_array_by_index(msg, 5, file, 100); // Pattern file name + put_uint8_t_by_index(msg, 105, detected); // Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 106, 90); } /** - * @brief Pack a pattern_detected message + * @brief Pack a pattern_detected message on a channel * @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 @@ -53,18 +63,46 @@ 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 char* 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) +{ + msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; + + put_float_by_index(msg, 0, confidence); // Confidence of detection + put_uint8_t_by_index(msg, 4, type); // 0: Pattern, 1: Letter + put_char_array_by_index(msg, 5, file, 100); // Pattern file name + put_uint8_t_by_index(msg, 105, detected); // Accepted as true detection, 0 no, 1 yes + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 106, 90); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a pattern_detected message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param type 0: Pattern, 1: Letter + * @param confidence Confidence of detection + * @param file Pattern file name + * @param detected Accepted as true detection, 0 no, 1 yes + */ +static inline void mavlink_msg_pattern_detected_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t type,float confidence,const char *file,uint8_t detected) { - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - 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 + put_float_by_index(msg, 0, confidence); // Confidence of detection + put_uint8_t_by_index(msg, 4, type); // 0: Pattern, 1: Letter + put_char_array_by_index(msg, 5, file, 100); // Pattern file name + put_uint8_t_by_index(msg, 105, detected); // Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); + mavlink_finalize_message_chan_send(msg, chan, 106, 90); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a pattern_detected struct into a message @@ -79,8 +117,6 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui return mavlink_msg_pattern_detected_pack(system_id, component_id, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a pattern_detected message * @param chan MAVLink channel to send the message @@ -90,38 +126,19 @@ 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 */ -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* 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_header_t hdr; - mavlink_pattern_detected_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN ) - payload.type = type; // uint8_t:0: Pattern, 1: Letter - payload.confidence = confidence; // float:Confidence of detection - memcpy(payload.file, file, sizeof(payload.file)); // char[100]:Pattern file name - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x34, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 106); + mavlink_msg_pattern_detected_pack_chan_send(chan, msg, type, confidence, file, detected); } #endif + // MESSAGE PATTERN_DETECTED UNPACKING + /** * @brief Get field type from pattern_detected message * @@ -129,8 +146,7 @@ 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) { - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -140,8 +156,7 @@ 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) { - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; - return (float)(p->confidence); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -149,12 +164,9 @@ 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, char* file) +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(file, p->file, sizeof(p->file)); - return sizeof(p->file); + return MAVLINK_MSG_RETURN_char_array(msg, file, 100, 5); } /** @@ -164,8 +176,7 @@ 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) { - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; - return (uint8_t)(p->detected); + return MAVLINK_MSG_RETURN_uint8_t(msg, 105); } /** @@ -176,5 +187,12 @@ 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) { - memcpy( pattern_detected, msg->payload, sizeof(mavlink_pattern_detected_t)); +#if MAVLINK_NEED_BYTE_SWAP + pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); + pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); + mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); + pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); +#else + memcpy(pattern_detected, MAVLINK_PAYLOAD(msg), 106); +#endif } 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 2f35b963680be4953439cec937c50805171df85c..d7a9522dd5db0262e967611a52580c5706ed4532 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -1,25 +1,39 @@ // 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 -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_KEY 0xA3 -#define MAVLINK_MSG_161_KEY 0xA3 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST 191 -typedef struct __mavlink_point_of_interest_t +typedef struct __mavlink_point_of_interest_t { - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - 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 - + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + 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_ID_POINT_OF_INTEREST_LEN 43 +#define MAVLINK_MSG_ID_191_LEN 43 + #define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 +#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST { \ + "POINT_OF_INTEREST", \ + 8, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_t, z) }, \ + { "timeout", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_point_of_interest_t, timeout) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_point_of_interest_t, type) }, \ + { "color", MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_point_of_interest_t, color) }, \ + { "coordinate_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_point_of_interest_t, coordinate_system) }, \ + { "name", MAVLINK_TYPE_CHAR, 26, 17, offsetof(mavlink_point_of_interest_t, name) }, \ + } \ +} + + /** * @brief Pack a point_of_interest message * @param system_id ID of this system @@ -36,25 +50,25 @@ 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 char* 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - 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 + put_float_by_index(msg, 0, x); // X Position + put_float_by_index(msg, 4, y); // Y Position + put_float_by_index(msg, 8, z); // Z Position + put_uint16_t_by_index(msg, 12, timeout); // 0: no timeout, >1: timeout in seconds + put_uint8_t_by_index(msg, 14, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + put_uint8_t_by_index(msg, 15, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + put_uint8_t_by_index(msg, 16, coordinate_system); // 0: global, 1:local + put_char_array_by_index(msg, 17, name, 26); // POI name - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 43, 95); } /** - * @brief Pack a point_of_interest message + * @brief Pack a point_of_interest message on a channel * @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 @@ -69,22 +83,58 @@ 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 char* 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) +{ + msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; + + put_float_by_index(msg, 0, x); // X Position + put_float_by_index(msg, 4, y); // Y Position + put_float_by_index(msg, 8, z); // Z Position + put_uint16_t_by_index(msg, 12, timeout); // 0: no timeout, >1: timeout in seconds + put_uint8_t_by_index(msg, 14, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + put_uint8_t_by_index(msg, 15, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + put_uint8_t_by_index(msg, 16, coordinate_system); // 0: global, 1:local + put_char_array_by_index(msg, 17, name, 26); // POI name + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 43, 95); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a point_of_interest message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + * @param coordinate_system 0: global, 1:local + * @param timeout 0: no timeout, >1: timeout in seconds + * @param x X Position + * @param y Y Position + * @param z Z Position + * @param name POI name + */ +static inline void mavlink_msg_point_of_interest_pack_chan_send(mavlink_channel_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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - 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 + put_float_by_index(msg, 0, x); // X Position + put_float_by_index(msg, 4, y); // Y Position + put_float_by_index(msg, 8, z); // Z Position + put_uint16_t_by_index(msg, 12, timeout); // 0: no timeout, >1: timeout in seconds + put_uint8_t_by_index(msg, 14, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + put_uint8_t_by_index(msg, 15, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + put_uint8_t_by_index(msg, 16, coordinate_system); // 0: global, 1:local + put_char_array_by_index(msg, 17, name, 26); // POI name - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); + mavlink_finalize_message_chan_send(msg, chan, 43, 95); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a point_of_interest struct into a message @@ -99,8 +149,6 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u return mavlink_msg_point_of_interest_pack(system_id, component_id, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a point_of_interest message * @param chan MAVLink channel to send the message @@ -114,42 +162,19 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param z Z Position * @param name POI name */ -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) +#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_header_t hdr; - mavlink_point_of_interest_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN ) - payload.type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - payload.color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - payload.coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - payload.timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - payload.x = x; // float:X Position - payload.y = y; // float:Y Position - payload.z = z; // float:Z Position - memcpy(payload.name, name, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA3, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 43); + mavlink_msg_point_of_interest_pack_chan_send(chan, msg, type, color, coordinate_system, timeout, x, y, z, name); } #endif + // MESSAGE POINT_OF_INTEREST UNPACKING + /** * @brief Get field type from point_of_interest message * @@ -157,8 +182,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 14); } /** @@ -168,8 +192,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (uint8_t)(p->color); + return MAVLINK_MSG_RETURN_uint8_t(msg, 15); } /** @@ -179,8 +202,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (uint8_t)(p->coordinate_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -190,8 +212,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (uint16_t)(p->timeout); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -201,8 +222,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -212,8 +232,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -223,8 +242,7 @@ 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) { - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -232,12 +250,9 @@ 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, char* name) +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(name, p->name, sizeof(p->name)); - return sizeof(p->name); + return MAVLINK_MSG_RETURN_char_array(msg, name, 26, 17); } /** @@ -248,5 +263,16 @@ 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) { - memcpy( point_of_interest, msg->payload, sizeof(mavlink_point_of_interest_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); + 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); + mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); +#else + memcpy(point_of_interest, MAVLINK_PAYLOAD(msg), 43); +#endif } 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 f5a470d6363fbb01bd0290752e89b580e1259545..3a2ec961b3ba36b5a4d28e29424fc66124cedfba 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,28 +1,45 @@ // 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 -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_KEY 0x10 -#define MAVLINK_MSG_162_KEY 0x10 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 192 -typedef struct __mavlink_point_of_interest_connection_t +typedef struct __mavlink_point_of_interest_connection_t { - float xp1; ///< X1 Position - float yp1; ///< Y1 Position - float zp1; ///< Z1 Position - float xp2; ///< X2 Position - float yp2; ///< Y2 Position - float zp2; ///< Z2 Position - 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 - + float xp1; ///< X1 Position + float yp1; ///< Y1 Position + float zp1; ///< Z1 Position + float xp2; ///< X2 Position + float yp2; ///< Y2 Position + float zp2; ///< Z2 Position + 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_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 +#define MAVLINK_MSG_ID_192_LEN 55 + #define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 +#define MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION { \ + "POINT_OF_INTEREST_CONNECTION", \ + 11, \ + { { "xp1", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_point_of_interest_connection_t, xp1) }, \ + { "yp1", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_point_of_interest_connection_t, yp1) }, \ + { "zp1", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_point_of_interest_connection_t, zp1) }, \ + { "xp2", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_point_of_interest_connection_t, xp2) }, \ + { "yp2", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_point_of_interest_connection_t, yp2) }, \ + { "zp2", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_point_of_interest_connection_t, zp2) }, \ + { "timeout", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_point_of_interest_connection_t, timeout) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_point_of_interest_connection_t, type) }, \ + { "color", MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_point_of_interest_connection_t, color) }, \ + { "coordinate_system", MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_point_of_interest_connection_t, coordinate_system) }, \ + { "name", MAVLINK_TYPE_CHAR, 26, 29, offsetof(mavlink_point_of_interest_connection_t, name) }, \ + } \ +} + + /** * @brief Pack a point_of_interest_connection message * @param system_id ID of this system @@ -42,28 +59,28 @@ 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 char* 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) { - 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; - 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, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); + put_float_by_index(msg, 0, xp1); // X1 Position + put_float_by_index(msg, 4, yp1); // Y1 Position + put_float_by_index(msg, 8, zp1); // Z1 Position + put_float_by_index(msg, 12, xp2); // X2 Position + put_float_by_index(msg, 16, yp2); // Y2 Position + put_float_by_index(msg, 20, zp2); // Z2 Position + put_uint16_t_by_index(msg, 24, timeout); // 0: no timeout, >1: timeout in seconds + put_uint8_t_by_index(msg, 26, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + put_uint8_t_by_index(msg, 27, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + put_uint8_t_by_index(msg, 28, coordinate_system); // 0: global, 1:local + put_char_array_by_index(msg, 29, name, 26); // POI connection name + + return mavlink_finalize_message(msg, system_id, component_id, 55, 36); } /** - * @brief Pack a point_of_interest_connection message + * @brief Pack a point_of_interest_connection message on a channel * @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 @@ -81,26 +98,68 @@ 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 char* 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) { - 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; - 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, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); + put_float_by_index(msg, 0, xp1); // X1 Position + put_float_by_index(msg, 4, yp1); // Y1 Position + put_float_by_index(msg, 8, zp1); // Z1 Position + put_float_by_index(msg, 12, xp2); // X2 Position + put_float_by_index(msg, 16, yp2); // Y2 Position + put_float_by_index(msg, 20, zp2); // Z2 Position + put_uint16_t_by_index(msg, 24, timeout); // 0: no timeout, >1: timeout in seconds + put_uint8_t_by_index(msg, 26, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + put_uint8_t_by_index(msg, 27, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + put_uint8_t_by_index(msg, 28, coordinate_system); // 0: global, 1:local + put_char_array_by_index(msg, 29, name, 26); // POI connection name + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 55, 36); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a point_of_interest_connection message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + * @param coordinate_system 0: global, 1:local + * @param timeout 0: no timeout, >1: timeout in seconds + * @param xp1 X1 Position + * @param yp1 Y1 Position + * @param zp1 Z1 Position + * @param xp2 X2 Position + * @param yp2 Y2 Position + * @param zp2 Z2 Position + * @param name POI connection name + */ +static inline void mavlink_msg_point_of_interest_connection_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; + + put_float_by_index(msg, 0, xp1); // X1 Position + put_float_by_index(msg, 4, yp1); // Y1 Position + put_float_by_index(msg, 8, zp1); // Z1 Position + put_float_by_index(msg, 12, xp2); // X2 Position + put_float_by_index(msg, 16, yp2); // Y2 Position + put_float_by_index(msg, 20, zp2); // Z2 Position + put_uint16_t_by_index(msg, 24, timeout); // 0: no timeout, >1: timeout in seconds + put_uint8_t_by_index(msg, 26, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + put_uint8_t_by_index(msg, 27, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + put_uint8_t_by_index(msg, 28, coordinate_system); // 0: global, 1:local + put_char_array_by_index(msg, 29, name, 26); // POI connection name + + mavlink_finalize_message_chan_send(msg, chan, 55, 36); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a point_of_interest_connection struct into a message * @@ -114,8 +173,6 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s return mavlink_msg_point_of_interest_connection_pack(system_id, component_id, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a point_of_interest_connection message * @param chan MAVLink channel to send the message @@ -132,45 +189,19 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param zp2 Z2 Position * @param name POI connection name */ -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) +#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_header_t hdr; - mavlink_point_of_interest_connection_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN ) - payload.type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - payload.color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - payload.coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - payload.timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - payload.xp1 = xp1; // float:X1 Position - payload.yp1 = yp1; // float:Y1 Position - payload.zp1 = zp1; // float:Z1 Position - payload.xp2 = xp2; // float:X2 Position - payload.yp2 = yp2; // float:Y2 Position - payload.zp2 = zp2; // float:Z2 Position - memcpy(payload.name, name, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x10, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 55); + mavlink_msg_point_of_interest_connection_pack_chan_send(chan, msg, type, color, coordinate_system, timeout, xp1, yp1, zp1, xp2, yp2, zp2, name); } #endif + // MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING + /** * @brief Get field type from point_of_interest_connection message * @@ -178,8 +209,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (uint8_t)(p->type); + return MAVLINK_MSG_RETURN_uint8_t(msg, 26); } /** @@ -189,8 +219,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (uint8_t)(p->color); + return MAVLINK_MSG_RETURN_uint8_t(msg, 27); } /** @@ -200,8 +229,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (uint8_t)(p->coordinate_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 28); } /** @@ -211,8 +239,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (uint16_t)(p->timeout); + return MAVLINK_MSG_RETURN_uint16_t(msg, 24); } /** @@ -222,8 +249,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (float)(p->xp1); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -233,8 +259,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (float)(p->yp1); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -244,8 +269,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (float)(p->zp1); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -255,8 +279,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (float)(p->xp2); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -266,8 +289,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (float)(p->yp2); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -277,8 +299,7 @@ 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) { - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - return (float)(p->zp2); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -286,12 +307,9 @@ 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, char* name) +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(name, p->name, sizeof(p->name)); - return sizeof(p->name); + return MAVLINK_MSG_RETURN_char_array(msg, name, 26, 29); } /** @@ -302,5 +320,19 @@ 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) { - memcpy( point_of_interest_connection, msg->payload, sizeof(mavlink_point_of_interest_connection_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); + 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); + mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); +#else + memcpy(point_of_interest_connection, MAVLINK_PAYLOAD(msg), 55); +#endif } 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 4c273a20c6065f568ecf053ba0c8d7771d9d0ab3..ce8c045abfe2aa65cc452e90f5ed17031a4b051d 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,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_KEY 0xA -#define MAVLINK_MSG_154_KEY 0xA +#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 160 -typedef struct __mavlink_position_control_offset_set_t +typedef struct __mavlink_position_control_offset_set_t { - 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 - + 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; +#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN 18 +#define MAVLINK_MSG_ID_160_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_OFFSET_SET { \ + "POSITION_CONTROL_OFFSET_SET", \ + 6, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_offset_set_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_offset_set_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_offset_set_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_offset_set_t, yaw) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_position_control_offset_set_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_position_control_offset_set_t, target_component) }, \ + } \ +} + + /** * @brief Pack a position_control_offset_set message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_position_control_offset_set_t * @param yaw yaw orientation offset in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_float_by_index(msg, 0, x); // x position offset + put_float_by_index(msg, 4, y); // y position offset + put_float_by_index(msg, 8, z); // z position offset + put_float_by_index(msg, 12, yaw); // yaw orientation offset in radians, 0 = NORTH + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 244); } /** - * @brief Pack a position_control_offset_set message + * @brief Pack a position_control_offset_set message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t syst * @param yaw yaw orientation offset in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; + + put_float_by_index(msg, 0, x); // x position offset + put_float_by_index(msg, 4, y); // y position offset + put_float_by_index(msg, 8, z); // z position offset + put_float_by_index(msg, 12, yaw); // yaw orientation offset in radians, 0 = NORTH + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 244); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a position_control_offset_set message on a channel and send + * @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 x x position offset + * @param y y position offset + * @param z z position offset + * @param yaw yaw orientation offset in radians, 0 = NORTH + */ +static inline void mavlink_msg_position_control_offset_set_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,float x,float y,float z,float yaw) { - 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; - 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 + put_float_by_index(msg, 0, x); // x position offset + put_float_by_index(msg, 4, y); // y position offset + put_float_by_index(msg, 8, z); // z position offset + put_float_by_index(msg, 12, yaw); // yaw orientation offset in radians, 0 = NORTH + put_uint8_t_by_index(msg, 16, target_system); // System ID + put_uint8_t_by_index(msg, 17, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 244); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a position_control_offset_set struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy return mavlink_msg_position_control_offset_set_pack(system_id, component_id, msg, position_control_offset_set->target_system, position_control_offset_set->target_component, position_control_offset_set->x, position_control_offset_set->y, position_control_offset_set->z, position_control_offset_set->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_control_offset_set message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_position_control_offset_set_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.x = x; // float:x position offset - payload.y = y; // float:y position offset - payload.z = z; // float:z position offset - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_position_control_offset_set_pack_chan_send(chan, msg, target_system, target_component, x, y, z, yaw); } #endif + // MESSAGE POSITION_CONTROL_OFFSET_SET UNPACKING + /** * @brief Get field target_system from position_control_offset_set message * @@ -142,8 +164,7 @@ 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) { - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 16); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 17); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( position_control_offset_set, msg->payload, sizeof(mavlink_position_control_offset_set_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + 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); +#else + memcpy(position_control_offset_set, MAVLINK_PAYLOAD(msg), 18); +#endif } 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 1b9a8c922fa33c88011f1a374a7024f0cb259c77..71a304fe12106d9a08eeb0dcd7e76ec419275bc7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -1,21 +1,33 @@ // 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 -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_KEY 0x72 -#define MAVLINK_MSG_121_KEY 0x72 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 170 -typedef struct __mavlink_position_control_setpoint_t +typedef struct __mavlink_position_control_setpoint_t { - 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 - + 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; +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 +#define MAVLINK_MSG_ID_170_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT { \ + "POSITION_CONTROL_SETPOINT", \ + 5, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_setpoint_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_setpoint_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_setpoint_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_setpoint_t, yaw) }, \ + { "id", MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_position_control_setpoint_t, id) }, \ + } \ +} + + /** * @brief Pack a position_control_setpoint message * @param system_id ID of this system @@ -29,22 +41,22 @@ typedef struct __mavlink_position_control_setpoint_t * @param yaw yaw orientation in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 28); } /** - * @brief Pack a position_control_setpoint message + * @brief Pack a position_control_setpoint message on a channel * @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 @@ -56,20 +68,50 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system * @param yaw yaw orientation in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 28); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a position_control_setpoint message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param id ID of waypoint, 0 for plain position + * @param x x position + * @param y y position + * @param z z position + * @param yaw yaw orientation in radians, 0 = NORTH + */ +static inline void mavlink_msg_position_control_setpoint_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t id,float x,float y,float z,float yaw) +{ + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; + + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position + + mavlink_finalize_message_chan_send(msg, chan, 18, 28); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a position_control_setpoint struct into a message * @@ -83,8 +125,6 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst return mavlink_msg_position_control_setpoint_pack(system_id, component_id, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_control_setpoint message * @param chan MAVLink channel to send the message @@ -95,39 +135,19 @@ 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 + 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_header_t hdr; - mavlink_position_control_setpoint_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN ) - payload.id = id; // uint16_t:ID of waypoint, 0 for plain position - payload.x = x; // float:x position - payload.y = y; // float:y position - payload.z = z; // float:z position - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x72, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_position_control_setpoint_pack_chan_send(chan, msg, id, x, y, z, yaw); } #endif + // MESSAGE POSITION_CONTROL_SETPOINT UNPACKING + /** * @brief Get field id from position_control_setpoint message * @@ -135,8 +155,7 @@ 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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; - return (uint16_t)(p->id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 16); } /** @@ -146,8 +165,7 @@ 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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -157,8 +175,7 @@ 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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -168,8 +185,7 @@ 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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -179,8 +195,7 @@ 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) { - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -191,5 +206,13 @@ 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) { - memcpy( position_control_setpoint, msg->payload, sizeof(mavlink_position_control_setpoint_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); +#else + memcpy(position_control_setpoint, MAVLINK_PAYLOAD(msg), 18); +#endif } 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 625ca74e17f43ec590cbaa0b234a55136e748905..8c9cd2bea846d35ded7f58393476a45a72d612d3 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,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_KEY 0xE1 -#define MAVLINK_MSG_120_KEY 0xE1 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 159 -typedef struct __mavlink_position_control_setpoint_set_t +typedef struct __mavlink_position_control_setpoint_set_t { - 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 - + 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; +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN 20 +#define MAVLINK_MSG_ID_159_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT_SET { \ + "POSITION_CONTROL_SETPOINT_SET", \ + 7, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_control_setpoint_set_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_control_setpoint_set_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_control_setpoint_set_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_control_setpoint_set_t, yaw) }, \ + { "id", MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_position_control_setpoint_set_t, id) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_position_control_setpoint_set_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_position_control_setpoint_set_t, target_component) }, \ + } \ +} + + /** * @brief Pack a position_control_setpoint_set message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_position_control_setpoint_set_t * @param yaw yaw orientation in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position + put_uint8_t_by_index(msg, 18, target_system); // System ID + put_uint8_t_by_index(msg, 19, target_component); // Component ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 11); } /** - * @brief Pack a position_control_setpoint_set message + * @brief Pack a position_control_setpoint_set message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t sy * @param yaw yaw orientation in radians, 0 = NORTH * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - 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; - 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 + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position + put_uint8_t_by_index(msg, 18, target_system); // System ID + put_uint8_t_by_index(msg, 19, target_component); // Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 11); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a position_control_setpoint_set message on a channel and send + * @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 id ID of waypoint, 0 for plain position + * @param x x position + * @param y y position + * @param z z position + * @param yaw yaw orientation in radians, 0 = NORTH + */ +static inline void mavlink_msg_position_control_setpoint_set_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + + put_float_by_index(msg, 0, x); // x position + put_float_by_index(msg, 4, y); // y position + put_float_by_index(msg, 8, z); // z position + put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH + put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position + put_uint8_t_by_index(msg, 18, target_system); // System ID + put_uint8_t_by_index(msg, 19, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 20, 11); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a position_control_setpoint_set struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t return mavlink_msg_position_control_setpoint_set_pack(system_id, component_id, msg, position_control_setpoint_set->target_system, position_control_setpoint_set->target_component, position_control_setpoint_set->id, position_control_setpoint_set->x, position_control_setpoint_set->y, position_control_setpoint_set->z, position_control_setpoint_set->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_control_setpoint_set message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_position_control_setpoint_set_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN ) - payload.target_system = target_system; // uint8_t:System ID - payload.target_component = target_component; // uint8_t:Component ID - payload.id = id; // uint16_t:ID of waypoint, 0 for plain position - payload.x = x; // float:x position - payload.y = y; // float:y position - payload.z = z; // float:z position - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xE1, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_position_control_setpoint_set_pack_chan_send(chan, msg, target_system, target_component, id, x, y, z, yaw); } #endif + // MESSAGE POSITION_CONTROL_SETPOINT_SET UNPACKING + /** * @brief Get field target_system from position_control_setpoint_set message * @@ -149,8 +173,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_system); + return MAVLINK_MSG_RETURN_uint8_t(msg, 18); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); + return MAVLINK_MSG_RETURN_uint8_t(msg, 19); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (uint16_t)(p->id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 16); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( position_control_setpoint_set, msg->payload, sizeof(mavlink_position_control_setpoint_set_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + position_control_setpoint_set->id = mavlink_msg_position_control_setpoint_set_get_id(msg); + 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); +#else + memcpy(position_control_setpoint_set, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 2341cb3437bd35af7dda15704ead1360b47b6bfa..ee7c5291905ad7da1edf2eb0a16818250ed6a9bf 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_RAW_AUX_KEY 0xAB -#define MAVLINK_MSG_141_KEY 0xAB +#define MAVLINK_MSG_ID_RAW_AUX 172 -typedef struct __mavlink_raw_aux_t +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) + 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) } mavlink_raw_aux_t; +#define MAVLINK_MSG_ID_RAW_AUX_LEN 16 +#define MAVLINK_MSG_ID_172_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_RAW_AUX { \ + "RAW_AUX", \ + 7, \ + { { "baro", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_raw_aux_t, baro) }, \ + { "adc1", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_raw_aux_t, adc1) }, \ + { "adc2", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_raw_aux_t, adc2) }, \ + { "adc3", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_raw_aux_t, adc3) }, \ + { "adc4", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_raw_aux_t, adc4) }, \ + { "vbat", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_raw_aux_t, vbat) }, \ + { "temp", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_aux_t, temp) }, \ + } \ +} + + /** * @brief Pack a raw_aux message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_raw_aux_t * @param baro Barometric pressure (hecto Pascal) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - 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) + put_int32_t_by_index(msg, 0, baro); // Barometric pressure (hecto Pascal) + put_uint16_t_by_index(msg, 4, adc1); // ADC1 (J405 ADC3, LPC2148 AD0.6) + put_uint16_t_by_index(msg, 6, adc2); // ADC2 (J405 ADC5, LPC2148 AD0.2) + put_uint16_t_by_index(msg, 8, adc3); // ADC3 (J405 ADC6, LPC2148 AD0.1) + put_uint16_t_by_index(msg, 10, adc4); // ADC4 (J405 ADC7, LPC2148 AD1.3) + put_uint16_t_by_index(msg, 12, vbat); // Battery voltage + put_int16_t_by_index(msg, 14, temp); // Temperature (degrees celcius) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 16, 182); } /** - * @brief Pack a raw_aux message + * @brief Pack a raw_aux message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo * @param baro Barometric pressure (hecto Pascal) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - 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) + put_int32_t_by_index(msg, 0, baro); // Barometric pressure (hecto Pascal) + put_uint16_t_by_index(msg, 4, adc1); // ADC1 (J405 ADC3, LPC2148 AD0.6) + put_uint16_t_by_index(msg, 6, adc2); // ADC2 (J405 ADC5, LPC2148 AD0.2) + put_uint16_t_by_index(msg, 8, adc3); // ADC3 (J405 ADC6, LPC2148 AD0.1) + put_uint16_t_by_index(msg, 10, adc4); // ADC4 (J405 ADC7, LPC2148 AD1.3) + put_uint16_t_by_index(msg, 12, vbat); // Battery voltage + put_int16_t_by_index(msg, 14, temp); // Temperature (degrees celcius) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 182); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a raw_aux message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) + * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) + * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) + * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) + * @param vbat Battery voltage + * @param temp Temperature (degrees celcius) + * @param baro Barometric pressure (hecto Pascal) + */ +static inline void mavlink_msg_raw_aux_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_RAW_AUX; + + put_int32_t_by_index(msg, 0, baro); // Barometric pressure (hecto Pascal) + put_uint16_t_by_index(msg, 4, adc1); // ADC1 (J405 ADC3, LPC2148 AD0.6) + put_uint16_t_by_index(msg, 6, adc2); // ADC2 (J405 ADC5, LPC2148 AD0.2) + put_uint16_t_by_index(msg, 8, adc3); // ADC3 (J405 ADC6, LPC2148 AD0.1) + put_uint16_t_by_index(msg, 10, adc4); // ADC4 (J405 ADC7, LPC2148 AD1.3) + put_uint16_t_by_index(msg, 12, vbat); // Battery voltage + put_int16_t_by_index(msg, 14, temp); // Temperature (degrees celcius) + + mavlink_finalize_message_chan_send(msg, chan, 16, 182); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a raw_aux struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com return mavlink_msg_raw_aux_pack(system_id, component_id, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a raw_aux message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_raw_aux_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RAW_AUX_LEN ) - payload.adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) - payload.adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) - payload.adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) - payload.adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) - payload.vbat = vbat; // uint16_t:Battery voltage - payload.temp = temp; // int16_t:Temperature (degrees celcius) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xAB, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 16); + mavlink_msg_raw_aux_pack_chan_send(chan, msg, adc1, adc2, adc3, adc4, vbat, temp, baro); } #endif + // MESSAGE RAW_AUX UNPACKING + /** * @brief Get field adc1 from raw_aux message * @@ -149,8 +173,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (uint16_t)(p->adc1); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (uint16_t)(p->adc2); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (uint16_t)(p->adc3); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (uint16_t)(p->adc4); + return MAVLINK_MSG_RETURN_uint16_t(msg, 10); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (uint16_t)(p->vbat); + return MAVLINK_MSG_RETURN_uint16_t(msg, 12); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (int16_t)(p->temp); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; - return (int32_t)(p->baro); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( raw_aux, msg->payload, sizeof(mavlink_raw_aux_t)); +#if MAVLINK_NEED_BYTE_SWAP + raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); + 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); +#else + memcpy(raw_aux, MAVLINK_PAYLOAD(msg), 16); +#endif } 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 47e8cdd306bdb3a0d2a39f3ab51c7abf96cc3af1..392cfec0b1393cd3d82c4c7ec230ea8983a78afb 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -1,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_KEY 0x24 -#define MAVLINK_MSG_100_KEY 0x24 +#define MAVLINK_MSG_ID_SET_CAM_SHUTTER 151 -typedef struct __mavlink_set_cam_shutter_t +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 - + 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 } mavlink_set_cam_shutter_t; +#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 +#define MAVLINK_MSG_ID_151_LEN 11 + + + +#define MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER { \ + "SET_CAM_SHUTTER", \ + 6, \ + { { "gain", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_set_cam_shutter_t, gain) }, \ + { "interval", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_set_cam_shutter_t, interval) }, \ + { "exposure", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_set_cam_shutter_t, exposure) }, \ + { "cam_no", MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_set_cam_shutter_t, cam_no) }, \ + { "cam_mode", MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_set_cam_shutter_t, cam_mode) }, \ + { "trigger_pin", MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_set_cam_shutter_t, trigger_pin) }, \ + } \ +} + + /** * @brief Pack a set_cam_shutter message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_set_cam_shutter_t * @param gain Camera gain * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - 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 + put_float_by_index(msg, 0, gain); // Camera gain + put_uint16_t_by_index(msg, 4, interval); // Shutter interval, in microseconds + put_uint16_t_by_index(msg, 6, exposure); // Exposure time, in microseconds + put_uint8_t_by_index(msg, 8, cam_no); // Camera id + put_uint8_t_by_index(msg, 9, cam_mode); // Camera mode: 0 = auto, 1 = manual + put_uint8_t_by_index(msg, 10, trigger_pin); // Trigger pin, 0-3 for PtGrey FireFly - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 11, 108); } /** - * @brief Pack a set_cam_shutter message + * @brief Pack a set_cam_shutter message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 * @param gain Camera gain * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; + + put_float_by_index(msg, 0, gain); // Camera gain + put_uint16_t_by_index(msg, 4, interval); // Shutter interval, in microseconds + put_uint16_t_by_index(msg, 6, exposure); // Exposure time, in microseconds + put_uint8_t_by_index(msg, 8, cam_no); // Camera id + put_uint8_t_by_index(msg, 9, cam_mode); // Camera mode: 0 = auto, 1 = manual + put_uint8_t_by_index(msg, 10, trigger_pin); // Trigger pin, 0-3 for PtGrey FireFly + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11, 108); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a set_cam_shutter message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param cam_no Camera id + * @param cam_mode Camera mode: 0 = auto, 1 = manual + * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly + * @param interval Shutter interval, in microseconds + * @param exposure Exposure time, in microseconds + * @param gain Camera gain + */ +static inline void mavlink_msg_set_cam_shutter_pack_chan_send(mavlink_channel_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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - 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 + put_float_by_index(msg, 0, gain); // Camera gain + put_uint16_t_by_index(msg, 4, interval); // Shutter interval, in microseconds + put_uint16_t_by_index(msg, 6, exposure); // Exposure time, in microseconds + put_uint8_t_by_index(msg, 8, cam_no); // Camera id + put_uint8_t_by_index(msg, 9, cam_mode); // Camera mode: 0 = auto, 1 = manual + put_uint8_t_by_index(msg, 10, trigger_pin); // Trigger pin, 0-3 for PtGrey FireFly - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); + mavlink_finalize_message_chan_send(msg, chan, 11, 108); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a set_cam_shutter struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin return mavlink_msg_set_cam_shutter_pack(system_id, component_id, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_cam_shutter message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_set_cam_shutter_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN ) - payload.cam_no = cam_no; // uint8_t:Camera id - payload.cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual - payload.trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly - payload.interval = interval; // uint16_t:Shutter interval, in microseconds - payload.exposure = exposure; // uint16_t:Exposure time, in microseconds - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x24, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 11); + mavlink_msg_set_cam_shutter_pack_chan_send(chan, msg, cam_no, cam_mode, trigger_pin, interval, exposure, gain); } #endif + // MESSAGE SET_CAM_SHUTTER UNPACKING + /** * @brief Get field cam_no from set_cam_shutter message * @@ -142,8 +164,7 @@ 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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; - return (uint8_t)(p->cam_no); + return MAVLINK_MSG_RETURN_uint8_t(msg, 8); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; - return (uint8_t)(p->cam_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 9); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; - return (uint8_t)(p->trigger_pin); + return MAVLINK_MSG_RETURN_uint8_t(msg, 10); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; - return (uint16_t)(p->interval); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; - return (uint16_t)(p->exposure); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; - return (float)(p->gain); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( set_cam_shutter, msg->payload, sizeof(mavlink_set_cam_shutter_t)); +#if MAVLINK_NEED_BYTE_SWAP + set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(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->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); +#else + memcpy(set_cam_shutter, MAVLINK_PAYLOAD(msg), 11); +#endif } 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 ec76297c9ef501138998547d8c9fbe83d4c8ca6a..bbc5d7750d195c33f9055a64b889c1c353e901cc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_KEY 0xDA -#define MAVLINK_MSG_112_KEY 0xDA +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 157 -typedef struct __mavlink_vicon_position_estimate_t +typedef struct __mavlink_vicon_position_estimate_t { - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - + uint64_t usec; ///< Timestamp (milliseconds) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad } mavlink_vicon_position_estimate_t; +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_157_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ + "VICON_POSITION_ESTIMATE", \ + 7, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vicon_position_estimate_t, z) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ + } \ +} + + /** * @brief Pack a vicon_position_estimate message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_vicon_position_estimate_t * @param yaw Yaw angle in rad * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X position + put_float_by_index(msg, 12, y); // Global Y position + put_float_by_index(msg, 16, z); // Global Z position + put_float_by_index(msg, 20, roll); // Roll angle in rad + put_float_by_index(msg, 24, pitch); // Pitch angle in rad + put_float_by_index(msg, 28, yaw); // Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 56); } /** - * @brief Pack a vicon_position_estimate message + * @brief Pack a vicon_position_estimate message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i * @param yaw Yaw angle in rad * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X position + put_float_by_index(msg, 12, y); // Global Y position + put_float_by_index(msg, 16, z); // Global Z position + put_float_by_index(msg, 20, roll); // Roll angle in rad + put_float_by_index(msg, 24, pitch); // Pitch angle in rad + put_float_by_index(msg, 28, yaw); // Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 56); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a vicon_position_estimate message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (milliseconds) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + */ +static inline void mavlink_msg_vicon_position_estimate_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) +{ + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X position + put_float_by_index(msg, 12, y); // Global Y position + put_float_by_index(msg, 16, z); // Global Z position + put_float_by_index(msg, 20, roll); // Roll angle in rad + put_float_by_index(msg, 24, pitch); // Pitch angle in rad + put_float_by_index(msg, 28, yaw); // Yaw angle in rad + + mavlink_finalize_message_chan_send(msg, chan, 32, 56); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a vicon_position_estimate struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vicon_position_estimate message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_vicon_position_estimate_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN ) - payload.usec = usec; // uint64_t:Timestamp (milliseconds) - payload.x = x; // float:Global X position - payload.y = y; // float:Global Y position - payload.z = z; // float:Global Z position - payload.roll = roll; // float:Roll angle in rad - payload.pitch = pitch; // float:Pitch angle in rad - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xDA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_vicon_position_estimate_pack_chan_send(chan, msg, usec, x, y, z, roll, pitch, yaw); } #endif + // MESSAGE VICON_POSITION_ESTIMATE UNPACKING + /** * @brief Get field usec from vicon_position_estimate message * @@ -149,8 +173,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( vicon_position_estimate, msg->payload, sizeof(mavlink_vicon_position_estimate_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(vicon_position_estimate, MAVLINK_PAYLOAD(msg), 32); +#endif } 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 9ba6a9bd09bf67f6e1dec9d31f215163248e70cb..4f9b5a10f51f2f343b4ef20ffda0511f90274448 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_KEY 0xDA -#define MAVLINK_MSG_111_KEY 0xDA +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 156 -typedef struct __mavlink_vision_position_estimate_t +typedef struct __mavlink_vision_position_estimate_t { - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - + uint64_t usec; ///< Timestamp (milliseconds) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad } mavlink_vision_position_estimate_t; +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_156_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ + "VISION_POSITION_ESTIMATE", \ + 7, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_position_estimate_t, z) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ + } \ +} + + /** * @brief Pack a vision_position_estimate message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_vision_position_estimate_t * @param yaw Yaw angle in rad * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X position + put_float_by_index(msg, 12, y); // Global Y position + put_float_by_index(msg, 16, z); // Global Z position + put_float_by_index(msg, 20, roll); // Roll angle in rad + put_float_by_index(msg, 24, pitch); // Pitch angle in rad + put_float_by_index(msg, 28, yaw); // Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 158); } /** - * @brief Pack a vision_position_estimate message + * @brief Pack a vision_position_estimate message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ * @param yaw Yaw angle in rad * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X position + put_float_by_index(msg, 12, y); // Global Y position + put_float_by_index(msg, 16, z); // Global Z position + put_float_by_index(msg, 20, roll); // Roll angle in rad + put_float_by_index(msg, 24, pitch); // Pitch angle in rad + put_float_by_index(msg, 28, yaw); // Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 158); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a vision_position_estimate message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (milliseconds) + * @param x Global X position + * @param y Global Y position + * @param z Global Z position + * @param roll Roll angle in rad + * @param pitch Pitch angle in rad + * @param yaw Yaw angle in rad + */ +static inline void mavlink_msg_vision_position_estimate_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) +{ + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X position + put_float_by_index(msg, 12, y); // Global Y position + put_float_by_index(msg, 16, z); // Global Z position + put_float_by_index(msg, 20, roll); // Roll angle in rad + put_float_by_index(msg, 24, pitch); // Pitch angle in rad + put_float_by_index(msg, 28, yaw); // Yaw angle in rad + + mavlink_finalize_message_chan_send(msg, chan, 32, 158); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a vision_position_estimate struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vision_position_estimate message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_vision_position_estimate_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN ) - payload.usec = usec; // uint64_t:Timestamp (milliseconds) - payload.x = x; // float:Global X position - payload.y = y; // float:Global Y position - payload.z = z; // float:Global Z position - payload.roll = roll; // float:Roll angle in rad - payload.pitch = pitch; // float:Pitch angle in rad - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xDA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_vision_position_estimate_pack_chan_send(chan, msg, usec, x, y, z, roll, pitch, yaw); } #endif + // MESSAGE VISION_POSITION_ESTIMATE UNPACKING + /** * @brief Get field usec from vision_position_estimate message * @@ -149,8 +173,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (float)(p->roll); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (float)(p->pitch); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; - return (float)(p->yaw); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( vision_position_estimate, msg->payload, sizeof(mavlink_vision_position_estimate_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(vision_position_estimate, MAVLINK_PAYLOAD(msg), 32); +#endif } 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 181367cbe90f93ee84c0ae36e097c30aac923a77..0065c116ce707199e216b7d935ce7cba59afbacc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -1,20 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_KEY 0xEB -#define MAVLINK_MSG_113_KEY 0xEB +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 158 -typedef struct __mavlink_vision_speed_estimate_t +typedef struct __mavlink_vision_speed_estimate_t { - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X speed - float y; ///< Global Y speed - float z; ///< Global Z speed - + uint64_t usec; ///< Timestamp (milliseconds) + float x; ///< Global X speed + float y; ///< Global Y speed + float z; ///< Global Z speed } mavlink_vision_speed_estimate_t; +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_ID_158_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ + "VISION_SPEED_ESTIMATE", \ + 4, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ + } \ +} + + /** * @brief Pack a vision_speed_estimate message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_vision_speed_estimate_t * @param z Global Z speed * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X speed + put_float_by_index(msg, 12, y); // Global Y speed + put_float_by_index(msg, 16, z); // Global Z speed - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 20, 208); } /** - * @brief Pack a vision_speed_estimate message + * @brief Pack a vision_speed_estimate message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, * @param z Global Z speed * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X speed + put_float_by_index(msg, 12, y); // Global Y speed + put_float_by_index(msg, 16, z); // Global Z speed + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 208); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a vision_speed_estimate message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (milliseconds) + * @param x Global X speed + * @param y Global Y speed + * @param z Global Z speed + */ +static inline void mavlink_msg_vision_speed_estimate_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint64_t usec,float x,float y,float z) { - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - 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 + put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) + put_float_by_index(msg, 8, x); // Global X speed + put_float_by_index(msg, 12, y); // Global Y speed + put_float_by_index(msg, 16, z); // Global Z speed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); + mavlink_finalize_message_chan_send(msg, chan, 20, 208); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a vision_speed_estimate struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vision_speed_estimate message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) { - mavlink_header_t hdr; - mavlink_vision_speed_estimate_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN ) - payload.usec = usec; // uint64_t:Timestamp (milliseconds) - payload.x = x; // float:Global X speed - payload.y = y; // float:Global Y speed - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xEB, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 20); + mavlink_msg_vision_speed_estimate_pack_chan_send(chan, msg, usec, x, y, z); } #endif + // MESSAGE VISION_SPEED_ESTIMATE UNPACKING + /** * @brief Get field usec from vision_speed_estimate message * @@ -128,8 +146,7 @@ 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) { - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; - return (float)(p->x); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; - return (float)(p->y); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; - return (float)(p->z); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( vision_speed_estimate, msg->payload, sizeof(mavlink_vision_speed_estimate_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(vision_speed_estimate, MAVLINK_PAYLOAD(msg), 20); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index a7eca8c8dd15fc7c7e332e7f5b15f061d417719a..d1ddbab2905f8418d74bbd03b1063b07b0d71efa 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -1,20 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_KEY 0xA9 -#define MAVLINK_MSG_153_KEY 0xA9 +#define MAVLINK_MSG_ID_WATCHDOG_COMMAND 183 -typedef struct __mavlink_watchdog_command_t +typedef struct __mavlink_watchdog_command_t { - 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 - + 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; +#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 +#define MAVLINK_MSG_ID_183_LEN 6 + + + +#define MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND { \ + "WATCHDOG_COMMAND", \ + 4, \ + { { "watchdog_id", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_command_t, watchdog_id) }, \ + { "process_id", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_command_t, process_id) }, \ + { "target_system_id", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_watchdog_command_t, target_system_id) }, \ + { "command_id", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_watchdog_command_t, command_id) }, \ + } \ +} + + /** * @brief Pack a watchdog_command message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_watchdog_command_t * @param command_id Command ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - 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 + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_id); // Process ID + put_uint8_t_by_index(msg, 4, target_system_id); // Target system ID + put_uint8_t_by_index(msg, 5, command_id); // Command ID - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 6, 162); } /** - * @brief Pack a watchdog_command message + * @brief Pack a watchdog_command message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint * @param command_id Command ID * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; + + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_id); // Process ID + put_uint8_t_by_index(msg, 4, target_system_id); // Target system ID + put_uint8_t_by_index(msg, 5, command_id); // Command ID + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 162); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a watchdog_command message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system_id Target system ID + * @param watchdog_id Watchdog ID + * @param process_id Process ID + * @param command_id Command ID + */ +static inline void mavlink_msg_watchdog_command_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target_system_id,uint16_t watchdog_id,uint16_t process_id,uint8_t command_id) { - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - 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 + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_id); // Process ID + put_uint8_t_by_index(msg, 4, target_system_id); // Target system ID + put_uint8_t_by_index(msg, 5, command_id); // Command ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); + mavlink_finalize_message_chan_send(msg, chan, 6, 162); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a watchdog_command struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui return mavlink_msg_watchdog_command_pack(system_id, component_id, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_command message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + 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_header_t hdr; - mavlink_watchdog_command_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN ) - payload.target_system_id = target_system_id; // uint8_t:Target system ID - payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID - payload.process_id = process_id; // uint16_t:Process ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xA9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 6); + mavlink_msg_watchdog_command_pack_chan_send(chan, msg, target_system_id, watchdog_id, process_id, command_id); } #endif + // MESSAGE WATCHDOG_COMMAND UNPACKING + /** * @brief Get field target_system_id from watchdog_command message * @@ -128,8 +146,7 @@ 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) { - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; - return (uint8_t)(p->target_system_id); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; - return (uint16_t)(p->watchdog_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; - return (uint16_t)(p->process_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 2); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; - return (uint8_t)(p->command_id); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( watchdog_command, msg->payload, sizeof(mavlink_watchdog_command_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); + watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); +#else + memcpy(watchdog_command, MAVLINK_PAYLOAD(msg), 6); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index 1f3f84633a0e070aa1cb6dc778b728e8a99eb8a5..a85760dccaaefb4bce16aa9f148938fbaf21a80d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -1,18 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_KEY 0x5C -#define MAVLINK_MSG_150_KEY 0x5C +#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 180 -typedef struct __mavlink_watchdog_heartbeat_t +typedef struct __mavlink_watchdog_heartbeat_t { - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_count; ///< Number of processes - + uint16_t watchdog_id; ///< Watchdog ID + uint16_t process_count; ///< Number of processes } mavlink_watchdog_heartbeat_t; +#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 +#define MAVLINK_MSG_ID_180_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT { \ + "WATCHDOG_HEARTBEAT", \ + 2, \ + { { "watchdog_id", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_watchdog_heartbeat_t, watchdog_id) }, \ + { "process_count", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_watchdog_heartbeat_t, process_count) }, \ + } \ +} + + /** * @brief Pack a watchdog_heartbeat message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_watchdog_heartbeat_t * @param process_count Number of processes * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_count); // Number of processes - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 153); } /** - * @brief Pack a watchdog_heartbeat message + * @brief Pack a watchdog_heartbeat message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui * @param process_count Number of processes * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_count); // Number of processes + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 153); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a watchdog_heartbeat message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_id Watchdog ID + * @param process_count Number of processes + */ +static inline void mavlink_msg_watchdog_heartbeat_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t watchdog_id,uint16_t process_count) { - mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_count); // Number of processes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 4, 153); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a watchdog_heartbeat struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_heartbeat message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) { - mavlink_header_t hdr; - mavlink_watchdog_heartbeat_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN ) - payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x5C, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_watchdog_heartbeat_pack_chan_send(chan, msg, watchdog_id, process_count); } #endif + // MESSAGE WATCHDOG_HEARTBEAT UNPACKING + /** * @brief Get field watchdog_id from watchdog_heartbeat message * @@ -114,8 +128,7 @@ 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) { - mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; - return (uint16_t)(p->watchdog_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; - return (uint16_t)(p->process_count); + return MAVLINK_MSG_RETURN_uint16_t(msg, 2); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( watchdog_heartbeat, msg->payload, sizeof(mavlink_watchdog_heartbeat_t)); +#if MAVLINK_NEED_BYTE_SWAP + watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); + watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); +#else + memcpy(watchdog_heartbeat, MAVLINK_PAYLOAD(msg), 4); +#endif } 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 9c10c937cdfb57ed403f4e90122b1d2f6fdbf865..2bcc5ee5912662c493ebdea53aa74dbc575a2585 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -1,23 +1,34 @@ // 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 -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_KEY 0x63 -#define MAVLINK_MSG_151_KEY 0x63 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 181 -typedef struct __mavlink_watchdog_process_info_t +typedef struct __mavlink_watchdog_process_info_t { - int32_t timeout; ///< Timeout (seconds) - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - char name[100]; ///< Process name - char arguments[147]; ///< Process arguments - + int32_t timeout; ///< Timeout (seconds) + uint16_t watchdog_id; ///< Watchdog ID + uint16_t process_id; ///< Process ID + char name[100]; ///< Process name + char arguments[147]; ///< Process arguments } mavlink_watchdog_process_info_t; + +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 +#define MAVLINK_MSG_ID_181_LEN 255 + #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 +#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO { \ + "WATCHDOG_PROCESS_INFO", \ + 5, \ + { { "timeout", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_info_t, timeout) }, \ + { "watchdog_id", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_info_t, watchdog_id) }, \ + { "process_id", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_info_t, process_id) }, \ + { "name", MAVLINK_TYPE_CHAR, 100, 8, offsetof(mavlink_watchdog_process_info_t, name) }, \ + { "arguments", MAVLINK_TYPE_CHAR, 147, 108, offsetof(mavlink_watchdog_process_info_t, arguments) }, \ + } \ +} + + /** * @brief Pack a watchdog_process_info message * @param system_id ID of this system @@ -31,22 +42,22 @@ 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 char* name, const char* 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) { - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - 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) + put_int32_t_by_index(msg, 0, timeout); // Timeout (seconds) + put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 6, process_id); // Process ID + put_char_array_by_index(msg, 8, name, 100); // Process name + put_char_array_by_index(msg, 108, arguments, 147); // Process arguments - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 255, 16); } /** - * @brief Pack a watchdog_process_info message + * @brief Pack a watchdog_process_info message on a channel * @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 @@ -58,19 +69,49 @@ 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 char* name, const char* 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) +{ + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; + + put_int32_t_by_index(msg, 0, timeout); // Timeout (seconds) + put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 6, process_id); // Process ID + put_char_array_by_index(msg, 8, name, 100); // Process name + put_char_array_by_index(msg, 108, arguments, 147); // Process arguments + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 16); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a watchdog_process_info message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_id Watchdog ID + * @param process_id Process ID + * @param name Process name + * @param arguments Process arguments + * @param timeout Timeout (seconds) + */ +static inline void mavlink_msg_watchdog_process_info_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint16_t watchdog_id,uint16_t process_id,const char *name,const char *arguments,int32_t timeout) { - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - 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) + put_int32_t_by_index(msg, 0, timeout); // Timeout (seconds) + put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 6, process_id); // Process ID + put_char_array_by_index(msg, 8, name, 100); // Process name + put_char_array_by_index(msg, 108, arguments, 147); // Process arguments - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); + mavlink_finalize_message_chan_send(msg, chan, 255, 16); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a watchdog_process_info struct into a message @@ -85,8 +126,6 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i return mavlink_msg_watchdog_process_info_pack(system_id, component_id, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_process_info message * @param chan MAVLink channel to send the message @@ -97,39 +136,19 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param arguments Process arguments * @param timeout Timeout (seconds) */ -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) +#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_header_t hdr; - mavlink_watchdog_process_info_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN ) - payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID - payload.process_id = process_id; // uint16_t:Process ID - memcpy(payload.name, name, sizeof(payload.name)); // char[100]:Process name - memcpy(payload.arguments, arguments, sizeof(payload.arguments)); // char[147]:Process arguments - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x63, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 255); + mavlink_msg_watchdog_process_info_pack_chan_send(chan, msg, watchdog_id, process_id, name, arguments, timeout); } #endif + // MESSAGE WATCHDOG_PROCESS_INFO UNPACKING + /** * @brief Get field watchdog_id from watchdog_process_info message * @@ -137,8 +156,7 @@ 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) { - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - return (uint16_t)(p->watchdog_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -148,8 +166,7 @@ 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) { - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - return (uint16_t)(p->process_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -157,12 +174,9 @@ 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, char* name) +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(name, p->name, sizeof(p->name)); - return sizeof(p->name); + return MAVLINK_MSG_RETURN_char_array(msg, name, 100, 8); } /** @@ -170,12 +184,9 @@ 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, char* arguments) +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(arguments, p->arguments, sizeof(p->arguments)); - return sizeof(p->arguments); + return MAVLINK_MSG_RETURN_char_array(msg, arguments, 147, 108); } /** @@ -185,8 +196,7 @@ 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) { - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - return (int32_t)(p->timeout); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -197,5 +207,13 @@ 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) { - memcpy( watchdog_process_info, msg->payload, sizeof(mavlink_watchdog_process_info_t)); +#if MAVLINK_NEED_BYTE_SWAP + watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); + 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); +#else + memcpy(watchdog_process_info, MAVLINK_PAYLOAD(msg), 255); +#endif } 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 8faab57fdc459e721de8ed206fd1005bcfeed56e..32bd57429f5fd11fc16334ee13302ccdf1348f21 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -1,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_KEY 0x4 -#define MAVLINK_MSG_152_KEY 0x4 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 182 -typedef struct __mavlink_watchdog_process_status_t +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 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 } mavlink_watchdog_process_status_t; +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 +#define MAVLINK_MSG_ID_182_LEN 12 + + + +#define MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS { \ + "WATCHDOG_PROCESS_STATUS", \ + 6, \ + { { "pid", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_watchdog_process_status_t, pid) }, \ + { "watchdog_id", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_watchdog_process_status_t, watchdog_id) }, \ + { "process_id", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_watchdog_process_status_t, process_id) }, \ + { "crashes", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_watchdog_process_status_t, crashes) }, \ + { "state", MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_watchdog_process_status_t, state) }, \ + { "muted", MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_watchdog_process_status_t, muted) }, \ + } \ +} + + /** * @brief Pack a watchdog_process_status message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_watchdog_process_status_t * @param crashes Number of crashes * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - 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 + put_int32_t_by_index(msg, 0, pid); // PID + put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 6, process_id); // Process ID + put_uint16_t_by_index(msg, 8, crashes); // Number of crashes + put_uint8_t_by_index(msg, 10, state); // Is running / finished / suspended / crashed + put_uint8_t_by_index(msg, 11, muted); // Is muted - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 12, 29); } /** - * @brief Pack a watchdog_process_status message + * @brief Pack a watchdog_process_status message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i * @param crashes Number of crashes * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; + + put_int32_t_by_index(msg, 0, pid); // PID + put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 6, process_id); // Process ID + put_uint16_t_by_index(msg, 8, crashes); // Number of crashes + put_uint8_t_by_index(msg, 10, state); // Is running / finished / suspended / crashed + put_uint8_t_by_index(msg, 11, muted); // Is muted + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 29); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a watchdog_process_status message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_id Watchdog ID + * @param process_id Process ID + * @param state Is running / finished / suspended / crashed + * @param muted Is muted + * @param pid PID + * @param crashes Number of crashes + */ +static inline void mavlink_msg_watchdog_process_status_pack_chan_send(mavlink_channel_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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - 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 + put_int32_t_by_index(msg, 0, pid); // PID + put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 6, process_id); // Process ID + put_uint16_t_by_index(msg, 8, crashes); // Number of crashes + put_uint8_t_by_index(msg, 10, state); // Is running / finished / suspended / crashed + put_uint8_t_by_index(msg, 11, muted); // Is muted - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); + mavlink_finalize_message_chan_send(msg, chan, 12, 29); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a watchdog_process_status struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system return mavlink_msg_watchdog_process_status_pack(system_id, component_id, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_process_status message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_watchdog_process_status_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN ) - payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID - payload.process_id = process_id; // uint16_t:Process ID - payload.state = state; // uint8_t:Is running / finished / suspended / crashed - payload.muted = muted; // uint8_t:Is muted - payload.pid = pid; // int32_t:PID - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x4, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 12); + mavlink_msg_watchdog_process_status_pack_chan_send(chan, msg, watchdog_id, process_id, state, muted, pid, crashes); } #endif + // MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING + /** * @brief Get field watchdog_id from watchdog_process_status message * @@ -142,8 +164,7 @@ 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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; - return (uint16_t)(p->watchdog_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 4); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; - return (uint16_t)(p->process_id); + return MAVLINK_MSG_RETURN_uint16_t(msg, 6); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; - return (uint8_t)(p->state); + return MAVLINK_MSG_RETURN_uint8_t(msg, 10); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; - return (uint8_t)(p->muted); + return MAVLINK_MSG_RETURN_uint8_t(msg, 11); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; - return (int32_t)(p->pid); + return MAVLINK_MSG_RETURN_int32_t(msg, 0); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; - return (uint16_t)(p->crashes); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( watchdog_process_status, msg->payload, sizeof(mavlink_watchdog_process_status_t)); +#if MAVLINK_NEED_BYTE_SWAP + watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); + 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->crashes = mavlink_msg_watchdog_process_status_get_crashes(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); +#else + memcpy(watchdog_process_status, MAVLINK_PAYLOAD(msg), 12); +#endif } diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 69a3ef52ec79649620ac23ad440cabcaf516dc22..5c8d8164b67101a3179b462ab0d83acc0b6c7e98 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from pixhawk.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 20 2011, 11:19 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -10,39 +9,50 @@ extern "C" { #endif +// MESSAGE LENGTHS AND CRCS -#include "../mavlink_protocol.h" +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 32, 32, 20, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 8, 255, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51, 5} +#endif -#define MAVLINK_ENABLED_PIXHAWK +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {153, 114, 143, 191, 105, 217, 104, 119, 0, 0, 0, 186, 194, 8, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 185, 222, 23, 179, 136, 66, 126, 198, 147, 0, 252, 162, 215, 229, 205, 51, 80, 101, 213, 8, 229, 21, 214, 170, 14, 73, 50, 142, 15, 3, 100, 24, 141, 148, 0, 0, 0, 183, 126, 130, 0, 148, 21, 0, 52, 124, 0, 0, 241, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 250, 156, 0, 0, 0, 0, 0, 0, 0, 146, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 108, 86, 95, 224, 0, 158, 56, 208, 11, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 148, 223, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 15, 248, 63, 83, 127} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {}, {}, {}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {}, {}, {}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {}, {}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {}, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT_SET, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_OFFSET_SET, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#endif +#include "../protocol.h" + +#define MAVLINK_ENABLED_PIXHAWK #include "../common/common.h" + // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif // ENUM DEFINITIONS + /** @brief Content Types for data transmission handshake */ enum DATA_TYPES { - DATA_TYPE_JPEG_IMAGE=1, - DATA_TYPE_RAW_IMAGE=2, - DATA_TYPE_KINECT=3, - DATA_TYPES_ENUM_END + DATA_TYPE_JPEG_IMAGE=1, /* | */ + DATA_TYPE_RAW_IMAGE=2, /* | */ + DATA_TYPE_KINECT=3, /* | */ + DATA_TYPES_ENUM_END=4, /* | */ }; - // MESSAGE DEFINITIONS - -#include "./mavlink_msg_attitude_control.h" #include "./mavlink_msg_set_cam_shutter.h" #include "./mavlink_msg_image_triggered.h" #include "./mavlink_msg_image_trigger_control.h" @@ -55,7 +65,6 @@ enum DATA_TYPES #include "./mavlink_msg_position_control_setpoint.h" #include "./mavlink_msg_marker.h" #include "./mavlink_msg_raw_aux.h" -#include "./mavlink_msg_aux_status.h" #include "./mavlink_msg_watchdog_heartbeat.h" #include "./mavlink_msg_watchdog_process_info.h" #include "./mavlink_msg_watchdog_process_status.h" @@ -66,19 +75,9 @@ enum DATA_TYPES #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" #include "./mavlink_msg_brief_feature.h" - - -// MESSAGE CRC KEYS - -#undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 179, 218, 22, 76, 226, 126, 117, 186, 0, 144, 0, 249, 172, 16, 0, 0, 0, 0, 0, 0, 33, 34, 191, 55, 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, 20, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 80, 0, 127, 200, 0, 0, 212, 251, 20, 38, 22, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 18, 103, 59, 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, 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, 138, 43, 141, 211, 144 } - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 56, 42, 33, 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, 43, 55, 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, 36, 30, 14, 14, 51 } +#include "./mavlink_msg_attitude_control.h" #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // PIXHAWK_H diff --git a/thirdParty/mavlink/include/pixhawk/testsuite.h b/thirdParty/mavlink/include/pixhawk/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..9c56c96df3c7fd2756d5ae976411542b01a55ca1 --- /dev/null +++ b/thirdParty/mavlink/include/pixhawk/testsuite.h @@ -0,0 +1,742 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from pixhawk.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef PIXHAWK_TESTSUITE_H +#define PIXHAWK_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_common(uint8_t, uint8_t); +static void mavlink_test_pixhawk(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + mavlink_test_common(system_id, component_id); + mavlink_test_pixhawk(system_id, component_id); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_cam_shutter_t packet2, packet1 = { + 17.0, + 17443, + 17547, + 29, + 96, + 163, + }; + if (sizeof(packet2) != 11) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_set_cam_shutter_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_cam_shutter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_set_cam_shutter_pack(system_id, component_id, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); + mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ilen + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +typedef union { + uint8_t b[2]; + uint16_t i; +} generic_16bit; + +typedef union { + uint8_t b[4]; + uint32_t i; + float f; +} generic_32bit; + +typedef union { + uint8_t b[8]; + uint64_t i; + double d; +} generic_64bit; + +/** + * @brief Place an unsigned byte into the buffer + * + * @param b the byte to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer + */ +static inline void put_uint8_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint8_t b) +{ + msg->payload.u8[wire_offset] = b; +} /** - * @brief Initialize the communication stack + * @brief Place a signed byte into the buffer * - * This function has to be called before using commParseBuffer() to initialize the different status registers. + * @param b the byte to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer + */ +static inline void put_int8_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int8_t b) +{ + msg->payload.i8[wire_offset] = b; +} + +/** + * @brief Place a char into the buffer * - * @return Will initialize the different buffers and status registers. + * @param b the byte to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer */ -static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +static inline void put_char_by_index(mavlink_message_t *msg, uint8_t wire_offset, char b) { - 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; - } + msg->payload.c[wire_offset] = b; } -static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +/** + * @brief Place two unsigned bytes into the buffer + * + * @param b the bytes to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer + */ +static inline void put_uint16_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint16_t b) { -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +#if MAVLINK_NEED_BYTE_SWAP + generic_16bit g; + g.i = b; + msg->payload.u8[wire_offset+0] = g.b[1]; + msg->payload.u8[wire_offset+1] = g.b[0]; #else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; + msg->payload.u16[wire_offset/2] = b; #endif - - 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. + * @brief Place two signed bytes into the buffer * - * @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. + * @param b the bytes to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer + */ +static inline void put_int16_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int16_t b) +{ +#if MAVLINK_NEED_BYTE_SWAP + put_uint16_t_by_index(msg, wire_offset, b); +#else + msg->payload.i16[wire_offset/2] = b; +#endif +} + +/** + * @brief Place four unsigned bytes into the buffer * - * @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 + * @param b the bytes to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer */ -static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +static inline void put_uint32_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint32_t b) { - // 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; +#if MAVLINK_NEED_BYTE_SWAP + generic_32bit g; + g.i = b; + msg->payload.u8[wire_offset+0] = g.b[3]; + msg->payload.u8[wire_offset+1] = g.b[2]; + msg->payload.u8[wire_offset+2] = g.b[1]; + msg->payload.u8[wire_offset+3] = g.b[0]; +#else + msg->payload.u32[wire_offset/4] = b; +#endif } /** - * @brief Finalize a MAVLink message with channel assignment + * @brief Place four signed bytes into the buffer * - * 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 b the bytes to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer + */ +static inline void put_int32_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int32_t b) +{ +#if MAVLINK_NEED_BYTE_SWAP + put_uint32_t_by_index(msg, wire_offset, b); +#else + msg->payload.i32[wire_offset/4] = b; +#endif +} + +/** + * @brief Place four unsigned bytes into the buffer * - * @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 + * @param b the bytes to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer */ -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) +static inline void put_uint64_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint64_t b) { - // 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; +#if MAVLINK_NEED_BYTE_SWAP + generic_64bit r; + r.i = b; + msg->payload.u8[wire_offset+0] = r.b[7]; + msg->payload.u8[wire_offset+1] = r.b[6]; + msg->payload.u8[wire_offset+2] = r.b[5]; + msg->payload.u8[wire_offset+3] = r.b[4]; + msg->payload.u8[wire_offset+4] = r.b[3]; + msg->payload.u8[wire_offset+5] = r.b[2]; + msg->payload.u8[wire_offset+6] = r.b[1]; + msg->payload.u8[wire_offset+7] = r.b[0]; +#else + msg->payload.u64[wire_offset/8] = b; +#endif } /** - * @brief Pack a message to send it over a serial byte stream + * @brief Place four signed bytes into the buffer + * + * @param b the bytes to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer */ -static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +static inline void put_int64_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int64_t b) { - *(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; +#if MAVLINK_NEED_BYTE_SWAP + put_uint64_t_by_index(msg, wire_offset, b); +#else + msg->payload.i64[wire_offset/8] = b; +#endif } /** - * @brief Get the required buffer size for this message + * @brief Place a float into the buffer + * + * @param b the float to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer */ -static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +static inline void put_float_by_index(mavlink_message_t *msg, uint8_t wire_offset, float b) { - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +#if MAVLINK_NEED_BYTE_SWAP + generic_32bit g; + g.f = b; + put_uint32_t_by_index(msg, wire_offset, g.i); +#else + msg->payload.f[wire_offset/4] = b; +#endif } -union checksum_ { - uint16_t s; - uint8_t c[2]; -}; +/** + * @brief Place a double into the buffer + * + * @param b the double to add + * @param wire_offset the position in the packet + * @param buffer the packet buffer + */ +static inline void put_double_by_index(mavlink_message_t *msg, uint8_t wire_offset, double b) +{ +#if MAVLINK_NEED_BYTE_SWAP + generic_64bit g; + g.d = b; + put_uint64_t_by_index(msg, wire_offset, g.i); +#else + msg->payload.d[wire_offset/8] = b; +#endif +} -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; +/* + * Place a char array into a buffer + */ +static inline void put_char_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const char *b, uint8_t array_length) +{ + memcpy(&msg->payload.c[wire_offset], b, array_length); +} +/* + * Place a uint8_t array into a buffer + */ +static inline void put_uint8_t_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) +{ + memcpy(&msg->payload.u8[wire_offset], b, array_length); +} -static inline void mavlink_start_checksum(mavlink_message_t* msg) +/* + * Place a int8_t array into a buffer + */ +static inline void put_int8_t_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const int8_t *b, uint8_t array_length) { - union checksum_ ck; - crc_init(&(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; + memcpy(&msg->payload.i8[wire_offset], b, array_length); +} + +#if MAVLINK_NEED_BYTE_SWAP +#define PUT_ARRAY_BY_INDEX(TYPE, V) \ +static inline void put_ ## TYPE ##_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +{ \ + uint16_t i; \ + for (i=0; ipayload.V[wire_offset/sizeof(TYPE)], b, array_length*sizeof(TYPE)); \ } +#endif + +PUT_ARRAY_BY_INDEX(uint16_t, u16) +PUT_ARRAY_BY_INDEX(uint32_t, u32) +PUT_ARRAY_BY_INDEX(uint64_t, u64) +PUT_ARRAY_BY_INDEX(int16_t, i16) +PUT_ARRAY_BY_INDEX(int32_t, i32) +PUT_ARRAY_BY_INDEX(int64_t, i64) +PUT_ARRAY_BY_INDEX(float, f) +PUT_ARRAY_BY_INDEX(double, d) + +#define MAVLINK_MSG_RETURN_char(msg, wire_offset) msg->payload.c[wire_offset] +#define MAVLINK_MSG_RETURN_int8_t(msg, wire_offset) msg->payload.i8[wire_offset] +#define MAVLINK_MSG_RETURN_uint8_t(msg, wire_offset) msg->payload.u8[wire_offset] -static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +static inline uint16_t MAVLINK_MSG_RETURN_uint16_t(const mavlink_message_t *msg, uint8_t wire_offset) { - 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]; +#if MAVLINK_NEED_BYTE_SWAP + generic_16bit r; + r.b[1] = msg->payload.u8[wire_offset+0]; + r.b[0] = msg->payload.u8[wire_offset+1]; + return r.i; +#else + return msg->payload.u16[wire_offset/2]; +#endif } -/** - * 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 - */ -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 inline int16_t MAVLINK_MSG_RETURN_int16_t(const mavlink_message_t *msg, uint8_t wire_offset) { -#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]; +#if MAVLINK_NEED_BYTE_SWAP + return (int16_t)MAVLINK_MSG_RETURN_uint16_t(msg, wire_offset); #else - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + return msg->payload.i16[wire_offset/2]; #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_msg_lengths[c] ) - status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway - else ; +} + +static inline uint32_t MAVLINK_MSG_RETURN_uint32_t(const mavlink_message_t *msg, uint8_t wire_offset) +{ +#if MAVLINK_NEED_BYTE_SWAP + generic_32bit r; + r.b[3] = msg->payload.u8[wire_offset+0]; + r.b[2] = msg->payload.u8[wire_offset+1]; + r.b[1] = msg->payload.u8[wire_offset+2]; + r.b[0] = msg->payload.u8[wire_offset+3]; + return r.i; +#else + return msg->payload.u32[wire_offset/4]; #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; - } - 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)); - } - } - 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; - - // For future use - -// if (status->msg_received == 1) -// { -// if ( r_message != NULL ) -// { -// return r_message; -// } -// else -// { -// return rxmsg; -// } -// } -// else -// { -// return NULL; -// } - return status->msg_received; } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline int32_t MAVLINK_MSG_RETURN_int32_t(const mavlink_message_t *msg, uint8_t wire_offset) +{ +#if MAVLINK_NEED_BYTE_SWAP + return (int32_t)MAVLINK_MSG_RETURN_uint32_t(msg, wire_offset); +#else + return msg->payload.i32[wire_offset/4]; +#endif +} -// To make MAVLink work on your MCU, define a similar function +static inline uint64_t MAVLINK_MSG_RETURN_uint64_t(const mavlink_message_t *msg, uint8_t wire_offset) +{ +#if MAVLINK_NEED_BYTE_SWAP + generic_64bit r; + r.b[7] = msg->payload.u8[wire_offset+0]; + r.b[6] = msg->payload.u8[wire_offset+1]; + r.b[5] = msg->payload.u8[wire_offset+2]; + r.b[4] = msg->payload.u8[wire_offset+3]; + r.b[3] = msg->payload.u8[wire_offset+4]; + r.b[2] = msg->payload.u8[wire_offset+5]; + r.b[1] = msg->payload.u8[wire_offset+6]; + r.b[0] = msg->payload.u8[wire_offset+7]; + return r.i; +#else + return msg->payload.u64[wire_offset/8]; +#endif +} -/* +static inline int64_t MAVLINK_MSG_RETURN_int64_t(const mavlink_message_t *msg, uint8_t wire_offset) +{ +#if MAVLINK_NEED_BYTE_SWAP + return (int64_t)MAVLINK_MSG_RETURN_uint64_t(msg, wire_offset); +#else + return msg->payload.i64[wire_offset/8]; +#endif +} -#include "mavlink_types.h" +static inline float MAVLINK_MSG_RETURN_float(const mavlink_message_t *msg, uint8_t wire_offset) +{ +#if MAVLINK_NEED_BYTE_SWAP + generic_32bit r; + r.b[3] = msg->payload.u8[wire_offset+0]; + r.b[2] = msg->payload.u8[wire_offset+1]; + r.b[1] = msg->payload.u8[wire_offset+2]; + r.b[0] = msg->payload.u8[wire_offset+3]; + return r.f; +#else + return msg->payload.f[wire_offset/4]; +#endif +} -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +static inline double MAVLINK_MSG_RETURN_double(const mavlink_message_t *msg, uint8_t wire_offset) { - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } +#if MAVLINK_NEED_BYTE_SWAP + generic_64bit r; + r.b[7] = msg->payload.u8[wire_offset+0]; + r.b[6] = msg->payload.u8[wire_offset+1]; + r.b[5] = msg->payload.u8[wire_offset+2]; + r.b[4] = msg->payload.u8[wire_offset+3]; + r.b[3] = msg->payload.u8[wire_offset+4]; + r.b[2] = msg->payload.u8[wire_offset+5]; + r.b[1] = msg->payload.u8[wire_offset+6]; + r.b[0] = msg->payload.u8[wire_offset+7]; + return r.d; +#else + return msg->payload.d[wire_offset/8]; +#endif } -static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +static inline uint16_t MAVLINK_MSG_RETURN_char_array(const mavlink_message_t *msg, char *value, + uint8_t array_length, uint8_t wire_offset) { - // 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); + memcpy(value, &msg->payload.c[wire_offset], array_length); + return array_length; } -static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +static inline uint16_t MAVLINK_MSG_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, + uint8_t array_length, uint8_t wire_offset) { - // 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] ); - } + memcpy(value, &msg->payload.u8[wire_offset], array_length); + return array_length; +} + +static inline uint16_t MAVLINK_MSG_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, + uint8_t array_length, uint8_t wire_offset) +{ + memcpy(value, &msg->payload.i8[wire_offset], array_length); + return array_length; +} + +#if MAVLINK_NEED_BYTE_SWAP +#define RETURN_ARRAY_BY_INDEX(TYPE, V) \ +static inline uint16_t MAVLINK_MSG_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ + uint8_t array_length, uint8_t wire_offset) \ +{ \ + uint16_t i; \ + for (i=0; ipayload.V[wire_offset/sizeof(TYPE)], array_length*sizeof(TYPE)); \ + return array_length*sizeof(value[0]); \ } - */ -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_ */ +RETURN_ARRAY_BY_INDEX(uint16_t, u16) +RETURN_ARRAY_BY_INDEX(uint32_t, u32) +RETURN_ARRAY_BY_INDEX(uint64_t, u64) +RETURN_ARRAY_BY_INDEX(int16_t, i16) +RETURN_ARRAY_BY_INDEX(int32_t, i32) +RETURN_ARRAY_BY_INDEX(int64_t, i64) +RETURN_ARRAY_BY_INDEX(float, f) +RETURN_ARRAY_BY_INDEX(double, d) + +#if MAVLINK_STACK_BUFFER == 1 +/* + we need to be able to declare a mavlink_message_t on the stack for + _send() calls. This can either be done by declaring a whole message, + or by declaring a buffer of just the right size for this specific + message type, and casting a mavlink_message_t structure to it. Using + the cast is a violation of the strict aliasing rules for C, so only + use it if you know its OK for your architecture + */ +#define MAVLINK_ALIGNED_MESSAGE(msg, length) \ +uint64_t _buf[(MAVLINK_NUM_CHECKSUM_BYTES+MAVLINK_NUM_NON_PAYLOAD_BYTES+(length)+7)/8]; \ +mavlink_message_t *msg = (mavlink_message_t *)&_buf[0] +#else +#define MAVLINK_ALIGNED_MESSAGE(msg, length) \ + mavlink_message_t _msg; \ + mavlink_message_t *msg = &_msg +#endif // MAVLINK_STACK_BUFFER + +#endif // _MAVLINK_PROTOCOL_H_ diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index 60f530b26274be971ab6a5de0427a2373e3228dd..8eb3f2357ef607d2b72bb8c67fde19e59394ba16 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,16 +1,23 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol built from slugs.xml * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, August 20 2011, 11:06 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" +#ifndef MAVLINK_STX +#define MAVLINK_STX 254 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif -#pragma pack(pop) + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 #endif + +#include "version.h" +#include "slugs.h" + +#endif // MAVLINK_H diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 51a3595c87c46bdf6f62126773303d9d8b5c98cf..7cea7c28fb2d7e271bfd57927afe9a6df8431b79 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_AIR_DATA_KEY 0x90 -#define MAVLINK_MSG_171_KEY 0x90 -typedef struct __mavlink_air_data_t +typedef struct __mavlink_air_data_t { - float dynamicPressure; ///< Dynamic pressure (Pa) - float staticPressure; ///< Static pressure (Pa) - uint16_t temperature; ///< Board temperature - + float dynamicPressure; ///< Dynamic pressure (Pa) + float staticPressure; ///< Static pressure (Pa) + uint16_t temperature; ///< Board temperature } mavlink_air_data_t; +#define MAVLINK_MSG_ID_AIR_DATA_LEN 10 +#define MAVLINK_MSG_ID_171_LEN 10 + + + +#define MAVLINK_MESSAGE_INFO_AIR_DATA { \ + "AIR_DATA", \ + 3, \ + { { "dynamicPressure", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_air_data_t, dynamicPressure) }, \ + { "staticPressure", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_air_data_t, staticPressure) }, \ + { "temperature", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_air_data_t, temperature) }, \ + } \ +} + + /** * @brief Pack a air_data message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_air_data_t * @param temperature Board temperature * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature + put_float_by_index(msg, 0, dynamicPressure); // Dynamic pressure (Pa) + put_float_by_index(msg, 4, staticPressure); // Static pressure (Pa) + put_uint16_t_by_index(msg, 8, temperature); // Board temperature - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIR_DATA_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 10, 232); } /** - * @brief Pack a air_data message + * @brief Pack a air_data message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t comp * @param temperature Board temperature * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature + put_float_by_index(msg, 0, dynamicPressure); // Dynamic pressure (Pa) + put_float_by_index(msg, 4, staticPressure); // Static pressure (Pa) + put_uint16_t_by_index(msg, 8, temperature); // Board temperature - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIR_DATA_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 232); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a air_data message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param dynamicPressure Dynamic pressure (Pa) + * @param staticPressure Static pressure (Pa) + * @param temperature Board temperature + */ +static inline void mavlink_msg_air_data_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float dynamicPressure,float staticPressure,uint16_t temperature) +{ + msg->msgid = MAVLINK_MSG_ID_AIR_DATA; + + put_float_by_index(msg, 0, dynamicPressure); // Dynamic pressure (Pa) + put_float_by_index(msg, 4, staticPressure); // Static pressure (Pa) + put_uint16_t_by_index(msg, 8, temperature); // Board temperature + + mavlink_finalize_message_chan_send(msg, chan, 10, 232); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a air_data struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co return mavlink_msg_air_data_pack(system_id, component_id, msg, air_data->dynamicPressure, air_data->staticPressure, air_data->temperature); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a air_data message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) { - mavlink_header_t hdr; - mavlink_air_data_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_AIR_DATA_LEN ) - payload.dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - payload.staticPressure = staticPressure; // float:Static pressure (Pa) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x90, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 10); + mavlink_msg_air_data_pack_chan_send(chan, msg, dynamicPressure, staticPressure, temperature); } #endif + // MESSAGE AIR_DATA UNPACKING + /** * @brief Get field dynamicPressure from air_data message * @@ -121,8 +137,7 @@ 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) { - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; - return (float)(p->dynamicPressure); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; - return (float)(p->staticPressure); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; - return (uint16_t)(p->temperature); + return MAVLINK_MSG_RETURN_uint16_t(msg, 8); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( air_data, msg->payload, sizeof(mavlink_air_data_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(air_data, MAVLINK_PAYLOAD(msg), 10); +#endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index f1915b2548de101fc9fb51def8d10418d03ae791..2eb43c1a554ea79643684d557c0fd36b37087c4a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_CPU_LOAD_KEY 0xCA -#define MAVLINK_MSG_170_KEY 0xCA -typedef struct __mavlink_cpu_load_t +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 + uint8_t sensLoad; ///< Sensor DSC Load + uint8_t ctrlLoad; ///< Control DSC Load } mavlink_cpu_load_t; +#define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 +#define MAVLINK_MSG_ID_170_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_CPU_LOAD { \ + "CPU_LOAD", \ + 3, \ + { { "batVolt", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cpu_load_t, batVolt) }, \ + { "sensLoad", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_cpu_load_t, sensLoad) }, \ + { "ctrlLoad", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_cpu_load_t, ctrlLoad) }, \ + } \ +} + + /** * @brief Pack a cpu_load message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_cpu_load_t * @param batVolt Battery Voltage in millivolts * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - 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 + put_uint16_t_by_index(msg, 0, batVolt); // Battery Voltage in millivolts + put_uint8_t_by_index(msg, 2, sensLoad); // Sensor DSC Load + put_uint8_t_by_index(msg, 3, ctrlLoad); // Control DSC Load - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CPU_LOAD_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 75); } /** - * @brief Pack a cpu_load message + * @brief Pack a cpu_load message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t comp * @param batVolt Battery Voltage in millivolts * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - 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 + put_uint16_t_by_index(msg, 0, batVolt); // Battery Voltage in millivolts + put_uint8_t_by_index(msg, 2, sensLoad); // Sensor DSC Load + put_uint8_t_by_index(msg, 3, ctrlLoad); // Control DSC Load - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CPU_LOAD_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 75); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a cpu_load message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param sensLoad Sensor DSC Load + * @param ctrlLoad Control DSC Load + * @param batVolt Battery Voltage in millivolts + */ +static inline void mavlink_msg_cpu_load_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t sensLoad,uint8_t ctrlLoad,uint16_t batVolt) +{ + msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; + + put_uint16_t_by_index(msg, 0, batVolt); // Battery Voltage in millivolts + put_uint8_t_by_index(msg, 2, sensLoad); // Sensor DSC Load + put_uint8_t_by_index(msg, 3, ctrlLoad); // Control DSC Load + + mavlink_finalize_message_chan_send(msg, chan, 4, 75); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a cpu_load struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co return mavlink_msg_cpu_load_pack(system_id, component_id, msg, cpu_load->sensLoad, cpu_load->ctrlLoad, cpu_load->batVolt); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a cpu_load message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - mavlink_header_t hdr; - mavlink_cpu_load_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CPU_LOAD_LEN ) - payload.sensLoad = sensLoad; // uint8_t:Sensor DSC Load - payload.ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xCA, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_cpu_load_pack_chan_send(chan, msg, sensLoad, ctrlLoad, batVolt); } #endif + // MESSAGE CPU_LOAD UNPACKING + /** * @brief Get field sensLoad from cpu_load message * @@ -121,8 +137,7 @@ 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) { - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; - return (uint8_t)(p->sensLoad); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; - return (uint8_t)(p->ctrlLoad); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; - return (uint16_t)(p->batVolt); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( cpu_load, msg->payload, sizeof(mavlink_cpu_load_t)); +#if MAVLINK_NEED_BYTE_SWAP + cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); + cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); + cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); +#else + memcpy(cpu_load, MAVLINK_PAYLOAD(msg), 4); +#endif } 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 442dae4da639598aaa8d07f883b430b94a54d058..551ca57e261c5e48f3d93cb73b3ca1b9171c881a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -1,18 +1,27 @@ // 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 -#define MAVLINK_MSG_ID_CTRL_SRFC_PT_KEY 0x35 -#define MAVLINK_MSG_181_KEY 0x35 -typedef struct __mavlink_ctrl_srfc_pt_t +typedef struct __mavlink_ctrl_srfc_pt_t { - uint16_t bitfieldPt; ///< Bitfield containing the PT configuration - 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; +#define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 +#define MAVLINK_MSG_ID_181_LEN 3 + + + +#define MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT { \ + "CTRL_SRFC_PT", \ + 2, \ + { { "bitfieldPt", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ctrl_srfc_pt_t, bitfieldPt) }, \ + { "target", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_ctrl_srfc_pt_t, target) }, \ + } \ +} + + /** * @brief Pack a ctrl_srfc_pt message * @param system_id ID of this system @@ -23,19 +32,19 @@ typedef struct __mavlink_ctrl_srfc_pt_t * @param bitfieldPt Bitfield containing the PT configuration * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + put_uint16_t_by_index(msg, 0, bitfieldPt); // Bitfield containing the PT configuration + put_uint8_t_by_index(msg, 2, target); // The system setting the commands - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 3, 104); } /** - * @brief Pack a ctrl_srfc_pt message + * @brief Pack a ctrl_srfc_pt message on a channel * @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 @@ -44,16 +53,40 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t * @param bitfieldPt Bitfield containing the PT configuration * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + + put_uint16_t_by_index(msg, 0, bitfieldPt); // Bitfield containing the PT configuration + put_uint8_t_by_index(msg, 2, target); // The system setting the commands + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a ctrl_srfc_pt message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the commands + * @param bitfieldPt Bitfield containing the PT configuration + */ +static inline void mavlink_msg_ctrl_srfc_pt_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint16_t bitfieldPt) { - mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + put_uint16_t_by_index(msg, 0, bitfieldPt); // Bitfield containing the PT configuration + put_uint8_t_by_index(msg, 2, target); // The system setting the commands - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); + mavlink_finalize_message_chan_send(msg, chan, 3, 104); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a ctrl_srfc_pt struct into a message @@ -68,8 +101,6 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ return mavlink_msg_ctrl_srfc_pt_pack(system_id, component_id, msg, ctrl_srfc_pt->target, ctrl_srfc_pt->bitfieldPt); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a ctrl_srfc_pt message * @param chan MAVLink channel to send the message @@ -77,36 +108,19 @@ 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 + static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) { - mavlink_header_t hdr; - mavlink_ctrl_srfc_pt_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN ) - payload.target = target; // uint8_t:The system setting the commands - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x35, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 3); + mavlink_msg_ctrl_srfc_pt_pack_chan_send(chan, msg, target, bitfieldPt); } #endif + // MESSAGE CTRL_SRFC_PT UNPACKING + /** * @brief Get field target from ctrl_srfc_pt message * @@ -114,8 +128,7 @@ 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) { - mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -125,8 +138,7 @@ 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) { - mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; - return (uint16_t)(p->bitfieldPt); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -137,5 +149,10 @@ 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) { - memcpy( ctrl_srfc_pt, msg->payload, sizeof(mavlink_ctrl_srfc_pt_t)); +#if MAVLINK_NEED_BYTE_SWAP + ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); + ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); +#else + memcpy(ctrl_srfc_pt, MAVLINK_PAYLOAD(msg), 3); +#endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index a77fa54f32e459d50891b4e1183681bf3dd14bc8..be2bef8e3e8fb9f7326b8e4835680f1a41a15325 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -1,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_DATA_LOG_KEY 0xB9 -#define MAVLINK_MSG_177_KEY 0xB9 -typedef struct __mavlink_data_log_t +typedef struct __mavlink_data_log_t { - float fl_1; ///< Log value 1 - float fl_2; ///< Log value 2 - float fl_3; ///< Log value 3 - float fl_4; ///< Log value 4 - float fl_5; ///< Log value 5 - float fl_6; ///< Log value 6 - + float fl_1; ///< Log value 1 + float fl_2; ///< Log value 2 + float fl_3; ///< Log value 3 + float fl_4; ///< Log value 4 + float fl_5; ///< Log value 5 + float fl_6; ///< Log value 6 } mavlink_data_log_t; +#define MAVLINK_MSG_ID_DATA_LOG_LEN 24 +#define MAVLINK_MSG_ID_177_LEN 24 + + + +#define MAVLINK_MESSAGE_INFO_DATA_LOG { \ + "DATA_LOG", \ + 6, \ + { { "fl_1", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_data_log_t, fl_1) }, \ + { "fl_2", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_data_log_t, fl_2) }, \ + { "fl_3", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_data_log_t, fl_3) }, \ + { "fl_4", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_data_log_t, fl_4) }, \ + { "fl_5", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_data_log_t, fl_5) }, \ + { "fl_6", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_data_log_t, fl_6) }, \ + } \ +} + + /** * @brief Pack a data_log message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_data_log_t * @param fl_6 Log value 6 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - 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 + put_float_by_index(msg, 0, fl_1); // Log value 1 + put_float_by_index(msg, 4, fl_2); // Log value 2 + put_float_by_index(msg, 8, fl_3); // Log value 3 + put_float_by_index(msg, 12, fl_4); // Log value 4 + put_float_by_index(msg, 16, fl_5); // Log value 5 + put_float_by_index(msg, 20, fl_6); // Log value 6 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_LOG_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 24, 167); } /** - * @brief Pack a data_log message + * @brief Pack a data_log message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t comp * @param fl_6 Log value 6 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_DATA_LOG; + + put_float_by_index(msg, 0, fl_1); // Log value 1 + put_float_by_index(msg, 4, fl_2); // Log value 2 + put_float_by_index(msg, 8, fl_3); // Log value 3 + put_float_by_index(msg, 12, fl_4); // Log value 4 + put_float_by_index(msg, 16, fl_5); // Log value 5 + put_float_by_index(msg, 20, fl_6); // Log value 6 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 167); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a data_log message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param fl_1 Log value 1 + * @param fl_2 Log value 2 + * @param fl_3 Log value 3 + * @param fl_4 Log value 4 + * @param fl_5 Log value 5 + * @param fl_6 Log value 6 + */ +static inline void mavlink_msg_data_log_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float fl_1,float fl_2,float fl_3,float fl_4,float fl_5,float fl_6) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - 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 + put_float_by_index(msg, 0, fl_1); // Log value 1 + put_float_by_index(msg, 4, fl_2); // Log value 2 + put_float_by_index(msg, 8, fl_3); // Log value 3 + put_float_by_index(msg, 12, fl_4); // Log value 4 + put_float_by_index(msg, 16, fl_5); // Log value 5 + put_float_by_index(msg, 20, fl_6); // Log value 6 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_LOG_LEN); + mavlink_finalize_message_chan_send(msg, chan, 24, 167); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a data_log struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co return mavlink_msg_data_log_pack(system_id, component_id, msg, data_log->fl_1, data_log->fl_2, data_log->fl_3, data_log->fl_4, data_log->fl_5, data_log->fl_6); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a data_log message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_data_log_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DATA_LOG_LEN ) - payload.fl_1 = fl_1; // float:Log value 1 - payload.fl_2 = fl_2; // float:Log value 2 - payload.fl_3 = fl_3; // float:Log value 3 - payload.fl_4 = fl_4; // float:Log value 4 - payload.fl_5 = fl_5; // float:Log value 5 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xB9, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 24); + mavlink_msg_data_log_pack_chan_send(chan, msg, fl_1, fl_2, fl_3, fl_4, fl_5, fl_6); } #endif + // MESSAGE DATA_LOG UNPACKING + /** * @brief Get field fl_1 from data_log message * @@ -142,8 +164,7 @@ 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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; - return (float)(p->fl_1); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; - return (float)(p->fl_2); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; - return (float)(p->fl_3); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; - return (float)(p->fl_4); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; - return (float)(p->fl_5); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; - return (float)(p->fl_6); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( data_log, msg->payload, sizeof(mavlink_data_log_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(data_log, MAVLINK_PAYLOAD(msg), 24); +#endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index 378edd1b4dda54107e1271e11661de59ac9db457..6567035867c59d5aa1ef6a8abb615590257591a6 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -1,22 +1,35 @@ // MESSAGE DIAGNOSTIC PACKING #define MAVLINK_MSG_ID_DIAGNOSTIC 173 -#define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 -#define MAVLINK_MSG_173_LEN 18 -#define MAVLINK_MSG_ID_DIAGNOSTIC_KEY 0xFE -#define MAVLINK_MSG_173_KEY 0xFE -typedef struct __mavlink_diagnostic_t +typedef struct __mavlink_diagnostic_t { - float diagFl1; ///< Diagnostic float 1 - float diagFl2; ///< Diagnostic float 2 - float diagFl3; ///< Diagnostic float 3 - int16_t diagSh1; ///< Diagnostic short 1 - int16_t diagSh2; ///< Diagnostic short 2 - int16_t diagSh3; ///< Diagnostic short 3 - + float diagFl1; ///< Diagnostic float 1 + float diagFl2; ///< Diagnostic float 2 + float diagFl3; ///< Diagnostic float 3 + int16_t diagSh1; ///< Diagnostic short 1 + int16_t diagSh2; ///< Diagnostic short 2 + int16_t diagSh3; ///< Diagnostic short 3 } mavlink_diagnostic_t; +#define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 +#define MAVLINK_MSG_ID_173_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_DIAGNOSTIC { \ + "DIAGNOSTIC", \ + 6, \ + { { "diagFl1", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_diagnostic_t, diagFl1) }, \ + { "diagFl2", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_diagnostic_t, diagFl2) }, \ + { "diagFl3", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_diagnostic_t, diagFl3) }, \ + { "diagSh1", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_diagnostic_t, diagSh1) }, \ + { "diagSh2", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_diagnostic_t, diagSh2) }, \ + { "diagSh3", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_diagnostic_t, diagSh3) }, \ + } \ +} + + /** * @brief Pack a diagnostic message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_diagnostic_t * @param diagSh3 Diagnostic short 3 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - 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 + put_float_by_index(msg, 0, diagFl1); // Diagnostic float 1 + put_float_by_index(msg, 4, diagFl2); // Diagnostic float 2 + put_float_by_index(msg, 8, diagFl3); // Diagnostic float 3 + put_int16_t_by_index(msg, 12, diagSh1); // Diagnostic short 1 + put_int16_t_by_index(msg, 14, diagSh2); // Diagnostic short 2 + put_int16_t_by_index(msg, 16, diagSh3); // Diagnostic short 3 - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 18, 2); } /** - * @brief Pack a diagnostic message + * @brief Pack a diagnostic message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t co * @param diagSh3 Diagnostic short 3 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; + + put_float_by_index(msg, 0, diagFl1); // Diagnostic float 1 + put_float_by_index(msg, 4, diagFl2); // Diagnostic float 2 + put_float_by_index(msg, 8, diagFl3); // Diagnostic float 3 + put_int16_t_by_index(msg, 12, diagSh1); // Diagnostic short 1 + put_int16_t_by_index(msg, 14, diagSh2); // Diagnostic short 2 + put_int16_t_by_index(msg, 16, diagSh3); // Diagnostic short 3 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 2); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a diagnostic message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param diagFl1 Diagnostic float 1 + * @param diagFl2 Diagnostic float 2 + * @param diagFl3 Diagnostic float 3 + * @param diagSh1 Diagnostic short 1 + * @param diagSh2 Diagnostic short 2 + * @param diagSh3 Diagnostic short 3 + */ +static inline void mavlink_msg_diagnostic_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float diagFl1,float diagFl2,float diagFl3,int16_t diagSh1,int16_t diagSh2,int16_t diagSh3) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - 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 + put_float_by_index(msg, 0, diagFl1); // Diagnostic float 1 + put_float_by_index(msg, 4, diagFl2); // Diagnostic float 2 + put_float_by_index(msg, 8, diagFl3); // Diagnostic float 3 + put_int16_t_by_index(msg, 12, diagSh1); // Diagnostic short 1 + put_int16_t_by_index(msg, 14, diagSh2); // Diagnostic short 2 + put_int16_t_by_index(msg, 16, diagSh3); // Diagnostic short 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); + mavlink_finalize_message_chan_send(msg, chan, 18, 2); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a diagnostic struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t return mavlink_msg_diagnostic_pack(system_id, component_id, msg, diagnostic->diagFl1, diagnostic->diagFl2, diagnostic->diagFl3, diagnostic->diagSh1, diagnostic->diagSh2, diagnostic->diagSh3); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a diagnostic message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_diagnostic_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN ) - payload.diagFl1 = diagFl1; // float:Diagnostic float 1 - payload.diagFl2 = diagFl2; // float:Diagnostic float 2 - payload.diagFl3 = diagFl3; // float:Diagnostic float 3 - payload.diagSh1 = diagSh1; // int16_t:Diagnostic short 1 - payload.diagSh2 = diagSh2; // int16_t:Diagnostic short 2 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xFE, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 18); + mavlink_msg_diagnostic_pack_chan_send(chan, msg, diagFl1, diagFl2, diagFl3, diagSh1, diagSh2, diagSh3); } #endif + // MESSAGE DIAGNOSTIC UNPACKING + /** * @brief Get field diagFl1 from diagnostic message * @@ -142,8 +164,7 @@ 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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; - return (float)(p->diagFl1); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; - return (float)(p->diagFl2); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; - return (float)(p->diagFl3); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; - return (int16_t)(p->diagSh1); + return MAVLINK_MSG_RETURN_int16_t(msg, 12); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; - return (int16_t)(p->diagSh2); + return MAVLINK_MSG_RETURN_int16_t(msg, 14); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; - return (int16_t)(p->diagSh3); + return MAVLINK_MSG_RETURN_int16_t(msg, 16); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( diagnostic, msg->payload, sizeof(mavlink_diagnostic_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(diagnostic, MAVLINK_PAYLOAD(msg), 18); +#endif } 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 2a0b8c5b6d440ceef8bdfc2491a620ccad6b1ba3..cef5fb0f8bdfa0058f5929ab4b9a426f8d261de6 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -1,23 +1,37 @@ // 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 -#define MAVLINK_MSG_ID_GPS_DATE_TIME_KEY 0xE -#define MAVLINK_MSG_179_KEY 0xE -typedef struct __mavlink_gps_date_time_t +typedef struct __mavlink_gps_date_time_t { - uint8_t year; ///< Year reported by Gps - uint8_t month; ///< Month reported by Gps - uint8_t day; ///< Day reported by Gps - uint8_t hour; ///< Hour reported by Gps - uint8_t min; ///< Min reported by Gps - uint8_t sec; ///< Sec reported by Gps - uint8_t visSat; ///< Visible sattelites reported by Gps - + uint8_t year; ///< Year reported by Gps + uint8_t month; ///< Month reported by Gps + uint8_t day; ///< Day reported by Gps + uint8_t hour; ///< Hour reported by Gps + uint8_t min; ///< Min reported by Gps + uint8_t sec; ///< Sec reported by Gps + uint8_t visSat; ///< Visible sattelites reported by Gps } mavlink_gps_date_time_t; +#define MAVLINK_MSG_ID_GPS_DATE_TIME_LEN 7 +#define MAVLINK_MSG_ID_179_LEN 7 + + + +#define MAVLINK_MESSAGE_INFO_GPS_DATE_TIME { \ + "GPS_DATE_TIME", \ + 7, \ + { { "year", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_date_time_t, year) }, \ + { "month", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_date_time_t, month) }, \ + { "day", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_date_time_t, day) }, \ + { "hour", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_gps_date_time_t, hour) }, \ + { "min", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_gps_date_time_t, min) }, \ + { "sec", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_gps_date_time_t, sec) }, \ + { "visSat", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_gps_date_time_t, visSat) }, \ + } \ +} + + /** * @brief Pack a gps_date_time message * @param system_id ID of this system @@ -33,24 +47,24 @@ typedef struct __mavlink_gps_date_time_t * @param visSat Visible sattelites reported by Gps * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - 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 + put_uint8_t_by_index(msg, 0, year); // Year reported by Gps + put_uint8_t_by_index(msg, 1, month); // Month reported by Gps + put_uint8_t_by_index(msg, 2, day); // Day reported by Gps + put_uint8_t_by_index(msg, 3, hour); // Hour reported by Gps + put_uint8_t_by_index(msg, 4, min); // Min reported by Gps + put_uint8_t_by_index(msg, 5, sec); // Sec reported by Gps + put_uint8_t_by_index(msg, 6, visSat); // Visible sattelites reported by Gps - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 7, 16); } /** - * @brief Pack a gps_date_time message + * @brief Pack a gps_date_time message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t * @param visSat Visible sattelites reported by Gps * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - 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 + put_uint8_t_by_index(msg, 0, year); // Year reported by Gps + put_uint8_t_by_index(msg, 1, month); // Month reported by Gps + put_uint8_t_by_index(msg, 2, day); // Day reported by Gps + put_uint8_t_by_index(msg, 3, hour); // Hour reported by Gps + put_uint8_t_by_index(msg, 4, min); // Min reported by Gps + put_uint8_t_by_index(msg, 5, sec); // Sec reported by Gps + put_uint8_t_by_index(msg, 6, visSat); // Visible sattelites reported by Gps - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 7, 16); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a gps_date_time message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param year Year reported by Gps + * @param month Month reported by Gps + * @param day Day reported by Gps + * @param hour Hour reported by Gps + * @param min Min reported by Gps + * @param sec Sec reported by Gps + * @param visSat Visible sattelites reported by Gps + */ +static inline void mavlink_msg_gps_date_time_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + + put_uint8_t_by_index(msg, 0, year); // Year reported by Gps + put_uint8_t_by_index(msg, 1, month); // Month reported by Gps + put_uint8_t_by_index(msg, 2, day); // Day reported by Gps + put_uint8_t_by_index(msg, 3, hour); // Hour reported by Gps + put_uint8_t_by_index(msg, 4, min); // Min reported by Gps + put_uint8_t_by_index(msg, 5, sec); // Sec reported by Gps + put_uint8_t_by_index(msg, 6, visSat); // Visible sattelites reported by Gps + + mavlink_finalize_message_chan_send(msg, chan, 7, 16); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a gps_date_time struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 return mavlink_msg_gps_date_time_pack(system_id, component_id, msg, gps_date_time->year, gps_date_time->month, gps_date_time->day, gps_date_time->hour, gps_date_time->min, gps_date_time->sec, gps_date_time->visSat); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_date_time message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_gps_date_time_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN ) - payload.year = year; // uint8_t:Year reported by Gps - payload.month = month; // uint8_t:Month reported by Gps - payload.day = day; // uint8_t:Day reported by Gps - payload.hour = hour; // uint8_t:Hour reported by Gps - payload.min = min; // uint8_t:Min reported by Gps - payload.sec = sec; // uint8_t:Sec reported by Gps - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xE, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 7); + mavlink_msg_gps_date_time_pack_chan_send(chan, msg, year, month, day, hour, min, sec, visSat); } #endif + // MESSAGE GPS_DATE_TIME UNPACKING + /** * @brief Get field year from gps_date_time message * @@ -149,8 +173,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->year); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->month); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->day); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->hour); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->min); + return MAVLINK_MSG_RETURN_uint8_t(msg, 4); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->sec); + return MAVLINK_MSG_RETURN_uint8_t(msg, 5); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; - return (uint8_t)(p->visSat); + return MAVLINK_MSG_RETURN_uint8_t(msg, 6); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( gps_date_time, msg->payload, sizeof(mavlink_gps_date_time_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(gps_date_time, MAVLINK_PAYLOAD(msg), 7); +#endif } 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 931a7e6c89a78da3d1b19646cb852f8c7c846ca5..46921f5b45c35ed3f4f357f4f74826cf2aefe229 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -1,20 +1,31 @@ // 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 -#define MAVLINK_MSG_ID_MID_LVL_CMDS_KEY 0x88 -#define MAVLINK_MSG_180_KEY 0x88 -typedef struct __mavlink_mid_lvl_cmds_t +typedef struct __mavlink_mid_lvl_cmds_t { - float hCommand; ///< Commanded Airspeed - float uCommand; ///< Log value 2 - float rCommand; ///< Log value 3 - 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; +#define MAVLINK_MSG_ID_MID_LVL_CMDS_LEN 13 +#define MAVLINK_MSG_ID_180_LEN 13 + + + +#define MAVLINK_MESSAGE_INFO_MID_LVL_CMDS { \ + "MID_LVL_CMDS", \ + 4, \ + { { "hCommand", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mid_lvl_cmds_t, hCommand) }, \ + { "uCommand", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mid_lvl_cmds_t, uCommand) }, \ + { "rCommand", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mid_lvl_cmds_t, rCommand) }, \ + { "target", MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mid_lvl_cmds_t, target) }, \ + } \ +} + + /** * @brief Pack a mid_lvl_cmds message * @param system_id ID of this system @@ -27,21 +38,21 @@ typedef struct __mavlink_mid_lvl_cmds_t * @param rCommand Log value 3 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - 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 + put_float_by_index(msg, 0, hCommand); // Commanded Airspeed + put_float_by_index(msg, 4, uCommand); // Log value 2 + put_float_by_index(msg, 8, rCommand); // Log value 3 + put_uint8_t_by_index(msg, 12, target); // The system setting the commands - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 13, 146); } /** - * @brief Pack a mid_lvl_cmds message + * @brief Pack a mid_lvl_cmds message on a channel * @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 @@ -52,18 +63,46 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t * @param rCommand Log value 3 * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; + + put_float_by_index(msg, 0, hCommand); // Commanded Airspeed + put_float_by_index(msg, 4, uCommand); // Log value 2 + put_float_by_index(msg, 8, rCommand); // Log value 3 + put_uint8_t_by_index(msg, 12, target); // The system setting the commands + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 146); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a mid_lvl_cmds message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the commands + * @param hCommand Commanded Airspeed + * @param uCommand Log value 2 + * @param rCommand Log value 3 + */ +static inline void mavlink_msg_mid_lvl_cmds_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,float hCommand,float uCommand,float rCommand) { - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - 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 + put_float_by_index(msg, 0, hCommand); // Commanded Airspeed + put_float_by_index(msg, 4, uCommand); // Log value 2 + put_float_by_index(msg, 8, rCommand); // Log value 3 + put_uint8_t_by_index(msg, 12, target); // The system setting the commands - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); + mavlink_finalize_message_chan_send(msg, chan, 13, 146); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a mid_lvl_cmds struct into a message @@ -78,8 +117,6 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ return mavlink_msg_mid_lvl_cmds_pack(system_id, component_id, msg, mid_lvl_cmds->target, mid_lvl_cmds->hCommand, mid_lvl_cmds->uCommand, mid_lvl_cmds->rCommand); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a mid_lvl_cmds message * @param chan MAVLink channel to send the message @@ -89,38 +126,19 @@ 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 + static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) { - mavlink_header_t hdr; - mavlink_mid_lvl_cmds_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN ) - payload.target = target; // uint8_t:The system setting the commands - payload.hCommand = hCommand; // float:Commanded Airspeed - payload.uCommand = uCommand; // float:Log value 2 - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x88, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 13); + mavlink_msg_mid_lvl_cmds_pack_chan_send(chan, msg, target, hCommand, uCommand, rCommand); } #endif + // MESSAGE MID_LVL_CMDS UNPACKING + /** * @brief Get field target from mid_lvl_cmds message * @@ -128,8 +146,7 @@ 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) { - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 12); } /** @@ -139,8 +156,7 @@ 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) { - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; - return (float)(p->hCommand); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -150,8 +166,7 @@ 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) { - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; - return (float)(p->uCommand); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -161,8 +176,7 @@ 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) { - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; - return (float)(p->rCommand); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -173,5 +187,12 @@ 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) { - memcpy( mid_lvl_cmds, msg->payload, sizeof(mavlink_mid_lvl_cmds_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); + mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); +#else + memcpy(mid_lvl_cmds, MAVLINK_PAYLOAD(msg), 13); +#endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index a3aeaa27c46300ef930d3c4a001d60740a505f57..8ce2800dea91a968010778fe0b2435e131f3951a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -1,22 +1,35 @@ // 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 -#define MAVLINK_MSG_ID_SENSOR_BIAS_KEY 0x6A -#define MAVLINK_MSG_172_KEY 0x6A -typedef struct __mavlink_sensor_bias_t +typedef struct __mavlink_sensor_bias_t { - float axBias; ///< Accelerometer X bias (m/s) - float ayBias; ///< Accelerometer Y bias (m/s) - float azBias; ///< Accelerometer Z bias (m/s) - float gxBias; ///< Gyro X bias (rad/s) - float gyBias; ///< Gyro Y bias (rad/s) - float gzBias; ///< Gyro Z bias (rad/s) - + float axBias; ///< Accelerometer X bias (m/s) + float ayBias; ///< Accelerometer Y bias (m/s) + float azBias; ///< Accelerometer Z bias (m/s) + float gxBias; ///< Gyro X bias (rad/s) + float gyBias; ///< Gyro Y bias (rad/s) + float gzBias; ///< Gyro Z bias (rad/s) } mavlink_sensor_bias_t; +#define MAVLINK_MSG_ID_SENSOR_BIAS_LEN 24 +#define MAVLINK_MSG_ID_172_LEN 24 + + + +#define MAVLINK_MESSAGE_INFO_SENSOR_BIAS { \ + "SENSOR_BIAS", \ + 6, \ + { { "axBias", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_bias_t, axBias) }, \ + { "ayBias", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sensor_bias_t, ayBias) }, \ + { "azBias", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sensor_bias_t, azBias) }, \ + { "gxBias", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_bias_t, gxBias) }, \ + { "gyBias", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_bias_t, gyBias) }, \ + { "gzBias", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_bias_t, gzBias) }, \ + } \ +} + + /** * @brief Pack a sensor_bias message * @param system_id ID of this system @@ -31,23 +44,23 @@ typedef struct __mavlink_sensor_bias_t * @param gzBias Gyro Z bias (rad/s) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - 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) + put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s) + put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s) + put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s) + put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s) + put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s) + put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 24, 168); } /** - * @brief Pack a sensor_bias message + * @brief Pack a sensor_bias message on a channel * @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 @@ -60,20 +73,52 @@ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t c * @param gzBias Gyro Z bias (rad/s) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) +{ + msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; + + put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s) + put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s) + put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s) + put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s) + put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s) + put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 168); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a sensor_bias message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param axBias Accelerometer X bias (m/s) + * @param ayBias Accelerometer Y bias (m/s) + * @param azBias Accelerometer Z bias (m/s) + * @param gxBias Gyro X bias (rad/s) + * @param gyBias Gyro Y bias (rad/s) + * @param gzBias Gyro Z bias (rad/s) + */ +static inline void mavlink_msg_sensor_bias_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + float axBias,float ayBias,float azBias,float gxBias,float gyBias,float gzBias) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - 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) + put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s) + put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s) + put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s) + put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s) + put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s) + put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); + mavlink_finalize_message_chan_send(msg, chan, 24, 168); } +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + /** * @brief Encode a sensor_bias struct into a message @@ -88,8 +133,6 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t return mavlink_msg_sensor_bias_pack(system_id, component_id, msg, sensor_bias->axBias, sensor_bias->ayBias, sensor_bias->azBias, sensor_bias->gxBias, sensor_bias->gyBias, sensor_bias->gzBias); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a sensor_bias message * @param chan MAVLink channel to send the message @@ -101,40 +144,19 @@ 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 + 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_header_t hdr; - mavlink_sensor_bias_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN ) - payload.axBias = axBias; // float:Accelerometer X bias (m/s) - payload.ayBias = ayBias; // float:Accelerometer Y bias (m/s) - payload.azBias = azBias; // float:Accelerometer Z bias (m/s) - payload.gxBias = gxBias; // float:Gyro X bias (rad/s) - payload.gyBias = gyBias; // float:Gyro Y bias (rad/s) - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x6A, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 24); + mavlink_msg_sensor_bias_pack_chan_send(chan, msg, axBias, ayBias, azBias, gxBias, gyBias, gzBias); } #endif + // MESSAGE SENSOR_BIAS UNPACKING + /** * @brief Get field axBias from sensor_bias message * @@ -142,8 +164,7 @@ 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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; - return (float)(p->axBias); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -153,8 +174,7 @@ 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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; - return (float)(p->ayBias); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -164,8 +184,7 @@ 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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; - return (float)(p->azBias); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -175,8 +194,7 @@ 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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; - return (float)(p->gxBias); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -186,8 +204,7 @@ 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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; - return (float)(p->gyBias); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -197,8 +214,7 @@ 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) { - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; - return (float)(p->gzBias); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -209,5 +225,14 @@ 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) { - memcpy( sensor_bias, msg->payload, sizeof(mavlink_sensor_bias_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(sensor_bias, MAVLINK_PAYLOAD(msg), 24); +#endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index 456d760e1ccc9a1731e8fcfe6ab490ddc419922c..134675de0d01fd4c9b36ae9515fd1605b26c15ea 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_SLUGS_ACTION_KEY 0xD4 -#define MAVLINK_MSG_183_KEY 0xD4 -typedef struct __mavlink_slugs_action_t +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 + 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 } mavlink_slugs_action_t; +#define MAVLINK_MSG_ID_SLUGS_ACTION_LEN 4 +#define MAVLINK_MSG_ID_183_LEN 4 + + + +#define MAVLINK_MESSAGE_INFO_SLUGS_ACTION { \ + "SLUGS_ACTION", \ + 3, \ + { { "actionVal", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_slugs_action_t, actionVal) }, \ + { "target", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_slugs_action_t, target) }, \ + { "actionId", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_slugs_action_t, actionId) }, \ + } \ +} + + /** * @brief Pack a slugs_action message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_slugs_action_t * @param actionVal Value associated with the action * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_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 + put_uint16_t_by_index(msg, 0, actionVal); // Value associated with the action + put_uint8_t_by_index(msg, 2, target); // The system reporting the action + put_uint8_t_by_index(msg, 3, actionId); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 4, 65); } /** - * @brief Pack a slugs_action message + * @brief Pack a slugs_action message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t * @param actionVal Value associated with the action * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_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 + put_uint16_t_by_index(msg, 0, actionVal); // Value associated with the action + put_uint8_t_by_index(msg, 2, target); // The system reporting the action + put_uint8_t_by_index(msg, 3, actionId); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 65); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a slugs_action message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system reporting the action + * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + * @param actionVal Value associated with the action + */ +static inline void mavlink_msg_slugs_action_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t actionId,uint16_t actionVal) +{ + msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + + put_uint16_t_by_index(msg, 0, actionVal); // Value associated with the action + put_uint8_t_by_index(msg, 2, target); // The system reporting the action + put_uint8_t_by_index(msg, 3, actionId); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + + mavlink_finalize_message_chan_send(msg, chan, 4, 65); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a slugs_action struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ return mavlink_msg_slugs_action_pack(system_id, component_id, msg, slugs_action->target, slugs_action->actionId, slugs_action->actionVal); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a slugs_action message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) { - mavlink_header_t hdr; - mavlink_slugs_action_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN ) - payload.target = target; // uint8_t:The system reporting the action - payload.actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xD4, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 4); + mavlink_msg_slugs_action_pack_chan_send(chan, msg, target, actionId, actionVal); } #endif + // MESSAGE SLUGS_ACTION UNPACKING + /** * @brief Get field target from slugs_action message * @@ -121,8 +137,7 @@ 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) { - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; - return (uint8_t)(p->target); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; - return (uint8_t)(p->actionId); + return MAVLINK_MSG_RETURN_uint8_t(msg, 3); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; - return (uint16_t)(p->actionVal); + return MAVLINK_MSG_RETURN_uint16_t(msg, 0); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( slugs_action, msg->payload, sizeof(mavlink_slugs_action_t)); +#if MAVLINK_NEED_BYTE_SWAP + slugs_action->actionVal = mavlink_msg_slugs_action_get_actionVal(msg); + slugs_action->target = mavlink_msg_slugs_action_get_target(msg); + slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); +#else + memcpy(slugs_action, MAVLINK_PAYLOAD(msg), 4); +#endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index f2b98db80b4ee90ae44b5aef7bf856d914f419f3..2ce93dd194ffec09baa36bc2a903f22893185a63 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -1,25 +1,41 @@ // 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 -#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_KEY 0xFF -#define MAVLINK_MSG_176_KEY 0xFF -typedef struct __mavlink_slugs_navigation_t +typedef struct __mavlink_slugs_navigation_t { - float u_m; ///< Measured Airspeed prior to the Nav Filter - float phi_c; ///< Commanded Roll - float theta_c; ///< Commanded Pitch - float psiDot_c; ///< Commanded Turn rate - float ay_body; ///< Y component of the body acceleration - float totalDist; ///< Total Distance to Run on this leg of Navigation - float dist2Go; ///< Remaining distance to Run on this leg of Navigation - uint8_t fromWP; ///< Origin WP - uint8_t toWP; ///< Destination WP - + float u_m; ///< Measured Airspeed prior to the Nav Filter + float phi_c; ///< Commanded Roll + float theta_c; ///< Commanded Pitch + float psiDot_c; ///< Commanded Turn rate + float ay_body; ///< Y component of the body acceleration + float totalDist; ///< Total Distance to Run on this leg of Navigation + float dist2Go; ///< Remaining distance to Run on this leg of Navigation + uint8_t fromWP; ///< Origin WP + uint8_t toWP; ///< Destination WP } mavlink_slugs_navigation_t; +#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN 30 +#define MAVLINK_MSG_ID_176_LEN 30 + + + +#define MAVLINK_MESSAGE_INFO_SLUGS_NAVIGATION { \ + "SLUGS_NAVIGATION", \ + 9, \ + { { "u_m", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_slugs_navigation_t, u_m) }, \ + { "phi_c", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_slugs_navigation_t, phi_c) }, \ + { "theta_c", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_slugs_navigation_t, theta_c) }, \ + { "psiDot_c", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_slugs_navigation_t, psiDot_c) }, \ + { "ay_body", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_slugs_navigation_t, ay_body) }, \ + { "totalDist", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_slugs_navigation_t, totalDist) }, \ + { "dist2Go", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_slugs_navigation_t, dist2Go) }, \ + { "fromWP", MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_slugs_navigation_t, fromWP) }, \ + { "toWP", MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_slugs_navigation_t, toWP) }, \ + } \ +} + + /** * @brief Pack a slugs_navigation message * @param system_id ID of this system @@ -37,26 +53,26 @@ typedef struct __mavlink_slugs_navigation_t * @param toWP Destination WP * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - 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, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); + put_float_by_index(msg, 0, u_m); // Measured Airspeed prior to the Nav Filter + put_float_by_index(msg, 4, phi_c); // Commanded Roll + put_float_by_index(msg, 8, theta_c); // Commanded Pitch + put_float_by_index(msg, 12, psiDot_c); // Commanded Turn rate + put_float_by_index(msg, 16, ay_body); // Y component of the body acceleration + put_float_by_index(msg, 20, totalDist); // Total Distance to Run on this leg of Navigation + put_float_by_index(msg, 24, dist2Go); // Remaining distance to Run on this leg of Navigation + put_uint8_t_by_index(msg, 28, fromWP); // Origin WP + put_uint8_t_by_index(msg, 29, toWP); // Destination WP + + return mavlink_finalize_message(msg, system_id, component_id, 30, 120); } /** - * @brief Pack a slugs_navigation message + * @brief Pack a slugs_navigation message on a channel * @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 @@ -72,24 +88,62 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint * @param toWP Destination WP * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - 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, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); + put_float_by_index(msg, 0, u_m); // Measured Airspeed prior to the Nav Filter + put_float_by_index(msg, 4, phi_c); // Commanded Roll + put_float_by_index(msg, 8, theta_c); // Commanded Pitch + put_float_by_index(msg, 12, psiDot_c); // Commanded Turn rate + put_float_by_index(msg, 16, ay_body); // Y component of the body acceleration + put_float_by_index(msg, 20, totalDist); // Total Distance to Run on this leg of Navigation + put_float_by_index(msg, 24, dist2Go); // Remaining distance to Run on this leg of Navigation + put_uint8_t_by_index(msg, 28, fromWP); // Origin WP + put_uint8_t_by_index(msg, 29, toWP); // Destination WP + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 120); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a slugs_navigation message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param u_m Measured Airspeed prior to the Nav Filter + * @param phi_c Commanded Roll + * @param theta_c Commanded Pitch + * @param psiDot_c Commanded Turn rate + * @param ay_body Y component of the body acceleration + * @param totalDist Total Distance to Run on this leg of Navigation + * @param dist2Go Remaining distance to Run on this leg of Navigation + * @param fromWP Origin WP + * @param toWP Destination WP + */ +static inline void mavlink_msg_slugs_navigation_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + + put_float_by_index(msg, 0, u_m); // Measured Airspeed prior to the Nav Filter + put_float_by_index(msg, 4, phi_c); // Commanded Roll + put_float_by_index(msg, 8, theta_c); // Commanded Pitch + put_float_by_index(msg, 12, psiDot_c); // Commanded Turn rate + put_float_by_index(msg, 16, ay_body); // Y component of the body acceleration + put_float_by_index(msg, 20, totalDist); // Total Distance to Run on this leg of Navigation + put_float_by_index(msg, 24, dist2Go); // Remaining distance to Run on this leg of Navigation + put_uint8_t_by_index(msg, 28, fromWP); // Origin WP + put_uint8_t_by_index(msg, 29, toWP); // Destination WP + + mavlink_finalize_message_chan_send(msg, chan, 30, 120); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a slugs_navigation struct into a message * @@ -103,8 +157,6 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui return mavlink_msg_slugs_navigation_pack(system_id, component_id, msg, slugs_navigation->u_m, slugs_navigation->phi_c, slugs_navigation->theta_c, slugs_navigation->psiDot_c, slugs_navigation->ay_body, slugs_navigation->totalDist, slugs_navigation->dist2Go, slugs_navigation->fromWP, slugs_navigation->toWP); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a slugs_navigation message * @param chan MAVLink channel to send the message @@ -119,43 +171,19 @@ 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 + 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_header_t hdr; - mavlink_slugs_navigation_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN ) - payload.u_m = u_m; // float:Measured Airspeed prior to the Nav Filter - payload.phi_c = phi_c; // float:Commanded Roll - payload.theta_c = theta_c; // float:Commanded Pitch - payload.psiDot_c = psiDot_c; // float:Commanded Turn rate - payload.ay_body = ay_body; // float:Y component of the body acceleration - payload.totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation - payload.dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation - payload.fromWP = fromWP; // uint8_t:Origin WP - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xFF, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 30); + mavlink_msg_slugs_navigation_pack_chan_send(chan, msg, u_m, phi_c, theta_c, psiDot_c, ay_body, totalDist, dist2Go, fromWP, toWP); } #endif + // MESSAGE SLUGS_NAVIGATION UNPACKING + /** * @brief Get field u_m from slugs_navigation message * @@ -163,8 +191,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->u_m); + return MAVLINK_MSG_RETURN_float(msg, 0); } /** @@ -174,8 +201,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->phi_c); + return MAVLINK_MSG_RETURN_float(msg, 4); } /** @@ -185,8 +211,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->theta_c); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -196,8 +221,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->psiDot_c); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -207,8 +231,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->ay_body); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -218,8 +241,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->totalDist); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -229,8 +251,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (float)(p->dist2Go); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -240,8 +261,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (uint8_t)(p->fromWP); + return MAVLINK_MSG_RETURN_uint8_t(msg, 28); } /** @@ -251,8 +271,7 @@ 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) { - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; - return (uint8_t)(p->toWP); + return MAVLINK_MSG_RETURN_uint8_t(msg, 29); } /** @@ -263,5 +282,17 @@ 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) { - memcpy( slugs_navigation, msg->payload, sizeof(mavlink_slugs_navigation_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(slugs_navigation, MAVLINK_PAYLOAD(msg), 30); +#endif } diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index eccf9d5e0dbd216238ef5c2ec26083fe896378c2..e67ee0193c9b58584982bbc918cd9f403160ff26 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from slugs.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 20 2011, 11:06 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -10,29 +9,42 @@ extern "C" { #endif +// MESSAGE LENGTHS AND CRCS -#include "../mavlink_protocol.h" +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 36, 30, 14, 14, 51, 5} +#endif -#define MAVLINK_ENABLED_SLUGS +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {153, 114, 143, 191, 105, 217, 104, 119, 0, 0, 0, 186, 194, 8, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 185, 222, 23, 179, 136, 66, 126, 198, 147, 0, 252, 162, 215, 229, 205, 51, 80, 101, 213, 8, 229, 21, 214, 170, 14, 73, 50, 142, 15, 3, 100, 24, 141, 148, 0, 0, 0, 183, 126, 130, 0, 148, 21, 0, 52, 124, 0, 0, 241, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 250, 156, 0, 0, 0, 0, 0, 0, 0, 146, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 75, 232, 168, 2, 0, 0, 120, 167, 0, 16, 146, 104, 0, 65, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 15, 248, 63, 83, 127} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {}, {}, {}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {}, {}, {}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {}, {}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_CPU_LOAD, MAVLINK_MESSAGE_INFO_AIR_DATA, MAVLINK_MESSAGE_INFO_SENSOR_BIAS, MAVLINK_MESSAGE_INFO_DIAGNOSTIC, {}, {}, MAVLINK_MESSAGE_INFO_SLUGS_NAVIGATION, MAVLINK_MESSAGE_INFO_DATA_LOG, {}, MAVLINK_MESSAGE_INFO_GPS_DATE_TIME, MAVLINK_MESSAGE_INFO_MID_LVL_CMDS, MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT, {}, MAVLINK_MESSAGE_INFO_SLUGS_ACTION, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#endif +#include "../protocol.h" + +#define MAVLINK_ENABLED_SLUGS #include "../common/common.h" + // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif // ENUM DEFINITIONS -// MESSAGE DEFINITIONS +// MESSAGE DEFINITIONS #include "./mavlink_msg_cpu_load.h" #include "./mavlink_msg_air_data.h" #include "./mavlink_msg_sensor_bias.h" @@ -44,18 +56,7 @@ extern "C" { #include "./mavlink_msg_ctrl_srfc_pt.h" #include "./mavlink_msg_slugs_action.h" - -// MESSAGE CRC KEYS - -#undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 179, 218, 22, 76, 226, 126, 117, 186, 0, 144, 0, 249, 172, 16, 0, 0, 0, 0, 0, 0, 33, 34, 191, 55, 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, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 80, 0, 127, 200, 0, 0, 212, 251, 20, 38, 22, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 18, 103, 59, 0, 0, 0, 0, 0, 0, 0, 74, 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, 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, 138, 43, 141, 211, 144 } - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 36, 30, 14, 14, 51 } - #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // SLUGS_H diff --git a/thirdParty/mavlink/include/slugs/testsuite.h b/thirdParty/mavlink/include/slugs/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..fc5d2aa122ded59471fa39e378652557ef150572 --- /dev/null +++ b/thirdParty/mavlink/include/slugs/testsuite.h @@ -0,0 +1,323 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from slugs.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef SLUGS_TESTSUITE_H +#define SLUGS_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_common(uint8_t, uint8_t); +static void mavlink_test_slugs(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + mavlink_test_common(system_id, component_id); + mavlink_test_slugs(system_id, component_id); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_cpu_load(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_cpu_load_t packet2, packet1 = { + 17235, + 139, + 206, + }; + if (sizeof(packet2) != 4) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_cpu_load_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_cpu_load_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_cpu_load_pack(system_id, component_id, &msg , packet1.sensLoad , packet1.ctrlLoad , packet1.batVolt ); + mavlink_msg_cpu_load_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sensLoad , packet1.ctrlLoad , packet1.batVolt ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; imsgid = MAVLINK_MSG_ID_TEST_TYPES; + + put_uint64_t_by_index(msg, 0, u64); // uint64_t + put_int64_t_by_index(msg, 8, s64); // int64_t + put_double_by_index(msg, 16, d); // double + put_uint64_t_array_by_index(msg, 24, u64_array, 3); // uint64_t_array + put_int64_t_array_by_index(msg, 48, s64_array, 3); // int64_t_array + put_double_array_by_index(msg, 72, d_array, 3); // double_array + put_uint32_t_by_index(msg, 96, u32); // uint32_t + put_int32_t_by_index(msg, 100, s32); // int32_t + put_float_by_index(msg, 104, f); // float + put_uint32_t_array_by_index(msg, 108, u32_array, 3); // uint32_t_array + put_int32_t_array_by_index(msg, 120, s32_array, 3); // int32_t_array + put_float_array_by_index(msg, 132, f_array, 3); // float_array + put_uint16_t_by_index(msg, 144, u16); // uint16_t + put_int16_t_by_index(msg, 146, s16); // int16_t + put_uint16_t_array_by_index(msg, 148, u16_array, 3); // uint16_t_array + put_int16_t_array_by_index(msg, 154, s16_array, 3); // int16_t_array + put_char_by_index(msg, 160, c); // char + put_char_array_by_index(msg, 161, s, 10); // string + put_uint8_t_by_index(msg, 171, u8); // uint8_t + put_int8_t_by_index(msg, 172, s8); // int8_t + put_uint8_t_array_by_index(msg, 173, u8_array, 3); // uint8_t_array + put_int8_t_array_by_index(msg, 176, s8_array, 3); // int8_t_array + + return mavlink_finalize_message(msg, system_id, component_id, 179, 103); +} + +/** + * @brief Pack a test_types message on a channel + * @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 c char + * @param s string + * @param u8 uint8_t + * @param u16 uint16_t + * @param u32 uint32_t + * @param u64 uint64_t + * @param s8 int8_t + * @param s16 int16_t + * @param s32 int32_t + * @param s64 int64_t + * @param f float + * @param d double + * @param u8_array uint8_t_array + * @param u16_array uint16_t_array + * @param u32_array uint32_t_array + * @param u64_array uint64_t_array + * @param s8_array int8_t_array + * @param s16_array int16_t_array + * @param s32_array int32_t_array + * @param s64_array int64_t_array + * @param f_array float_array + * @param d_array double_array + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array) +{ + msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; + + put_uint64_t_by_index(msg, 0, u64); // uint64_t + put_int64_t_by_index(msg, 8, s64); // int64_t + put_double_by_index(msg, 16, d); // double + put_uint64_t_array_by_index(msg, 24, u64_array, 3); // uint64_t_array + put_int64_t_array_by_index(msg, 48, s64_array, 3); // int64_t_array + put_double_array_by_index(msg, 72, d_array, 3); // double_array + put_uint32_t_by_index(msg, 96, u32); // uint32_t + put_int32_t_by_index(msg, 100, s32); // int32_t + put_float_by_index(msg, 104, f); // float + put_uint32_t_array_by_index(msg, 108, u32_array, 3); // uint32_t_array + put_int32_t_array_by_index(msg, 120, s32_array, 3); // int32_t_array + put_float_array_by_index(msg, 132, f_array, 3); // float_array + put_uint16_t_by_index(msg, 144, u16); // uint16_t + put_int16_t_by_index(msg, 146, s16); // int16_t + put_uint16_t_array_by_index(msg, 148, u16_array, 3); // uint16_t_array + put_int16_t_array_by_index(msg, 154, s16_array, 3); // int16_t_array + put_char_by_index(msg, 160, c); // char + put_char_array_by_index(msg, 161, s, 10); // string + put_uint8_t_by_index(msg, 171, u8); // uint8_t + put_int8_t_by_index(msg, 172, s8); // int8_t + put_uint8_t_array_by_index(msg, 173, u8_array, 3); // uint8_t_array + put_int8_t_array_by_index(msg, 176, s8_array, 3); // int8_t_array + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179, 103); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a test_types message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param c char + * @param s string + * @param u8 uint8_t + * @param u16 uint16_t + * @param u32 uint32_t + * @param u64 uint64_t + * @param s8 int8_t + * @param s16 int16_t + * @param s32 int32_t + * @param s64 int64_t + * @param f float + * @param d double + * @param u8_array uint8_t_array + * @param u16_array uint16_t_array + * @param u32_array uint32_t_array + * @param u64_array uint64_t_array + * @param s8_array int8_t_array + * @param s16_array int16_t_array + * @param s32_array int32_t_array + * @param s64_array int64_t_array + * @param f_array float_array + * @param d_array double_array + */ +static inline void mavlink_msg_test_types_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array) +{ + msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; + + put_uint64_t_by_index(msg, 0, u64); // uint64_t + put_int64_t_by_index(msg, 8, s64); // int64_t + put_double_by_index(msg, 16, d); // double + put_uint64_t_array_by_index(msg, 24, u64_array, 3); // uint64_t_array + put_int64_t_array_by_index(msg, 48, s64_array, 3); // int64_t_array + put_double_array_by_index(msg, 72, d_array, 3); // double_array + put_uint32_t_by_index(msg, 96, u32); // uint32_t + put_int32_t_by_index(msg, 100, s32); // int32_t + put_float_by_index(msg, 104, f); // float + put_uint32_t_array_by_index(msg, 108, u32_array, 3); // uint32_t_array + put_int32_t_array_by_index(msg, 120, s32_array, 3); // int32_t_array + put_float_array_by_index(msg, 132, f_array, 3); // float_array + put_uint16_t_by_index(msg, 144, u16); // uint16_t + put_int16_t_by_index(msg, 146, s16); // int16_t + put_uint16_t_array_by_index(msg, 148, u16_array, 3); // uint16_t_array + put_int16_t_array_by_index(msg, 154, s16_array, 3); // int16_t_array + put_char_by_index(msg, 160, c); // char + put_char_array_by_index(msg, 161, s, 10); // string + put_uint8_t_by_index(msg, 171, u8); // uint8_t + put_int8_t_by_index(msg, 172, s8); // int8_t + put_uint8_t_array_by_index(msg, 173, u8_array, 3); // uint8_t_array + put_int8_t_array_by_index(msg, 176, s8_array, 3); // int8_t_array + + mavlink_finalize_message_chan_send(msg, chan, 179, 103); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + +/** + * @brief Encode a test_types 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 test_types C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types) +{ + return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array); +} + +/** + * @brief Send a test_types message + * @param chan MAVLink channel to send the message + * + * @param c char + * @param s string + * @param u8 uint8_t + * @param u16 uint16_t + * @param u32 uint32_t + * @param u64 uint64_t + * @param s8 int8_t + * @param s16 int16_t + * @param s32 int32_t + * @param s64 int64_t + * @param f float + * @param d double + * @param u8_array uint8_t_array + * @param u16_array uint16_t_array + * @param u32_array uint32_t_array + * @param u64_array uint64_t_array + * @param s8_array int8_t_array + * @param s16_array int16_t_array + * @param s32_array int32_t_array + * @param s64_array int64_t_array + * @param f_array float_array + * @param d_array double_array + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array) +{ + MAVLINK_ALIGNED_MESSAGE(msg, 179); + mavlink_msg_test_types_pack_chan_send(chan, msg, c, s, u8, u16, u32, u64, s8, s16, s32, s64, f, d, u8_array, u16_array, u32_array, u64_array, s8_array, s16_array, s32_array, s64_array, f_array, d_array); +} + +#endif + +// MESSAGE TEST_TYPES UNPACKING + + +/** + * @brief Get field c from test_types message + * + * @return char + */ +static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_char(msg, 160); +} + +/** + * @brief Get field s from test_types message + * + * @return string + */ +static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s) +{ + return MAVLINK_MSG_RETURN_char_array(msg, s, 10, 161); +} + +/** + * @brief Get field u8 from test_types message + * + * @return uint8_t + */ +static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint8_t(msg, 171); +} + +/** + * @brief Get field u16 from test_types message + * + * @return uint16_t + */ +static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint16_t(msg, 144); +} + +/** + * @brief Get field u32 from test_types message + * + * @return uint32_t + */ +static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint32_t(msg, 96); +} + +/** + * @brief Get field u64 from test_types message + * + * @return uint64_t + */ +static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field s8 from test_types message + * + * @return int8_t + */ +static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_int8_t(msg, 172); +} + +/** + * @brief Get field s16 from test_types message + * + * @return int16_t + */ +static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_int16_t(msg, 146); +} + +/** + * @brief Get field s32 from test_types message + * + * @return int32_t + */ +static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_int32_t(msg, 100); +} + +/** + * @brief Get field s64 from test_types message + * + * @return int64_t + */ +static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_int64_t(msg, 8); +} + +/** + * @brief Get field f from test_types message + * + * @return float + */ +static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_float(msg, 104); +} + +/** + * @brief Get field d from test_types message + * + * @return double + */ +static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg) +{ + return MAVLINK_MSG_RETURN_double(msg, 16); +} + +/** + * @brief Get field u8_array from test_types message + * + * @return uint8_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array) +{ + return MAVLINK_MSG_RETURN_uint8_t_array(msg, u8_array, 3, 173); +} + +/** + * @brief Get field u16_array from test_types message + * + * @return uint16_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array) +{ + return MAVLINK_MSG_RETURN_uint16_t_array(msg, u16_array, 3, 148); +} + +/** + * @brief Get field u32_array from test_types message + * + * @return uint32_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array) +{ + return MAVLINK_MSG_RETURN_uint32_t_array(msg, u32_array, 3, 108); +} + +/** + * @brief Get field u64_array from test_types message + * + * @return uint64_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array) +{ + return MAVLINK_MSG_RETURN_uint64_t_array(msg, u64_array, 3, 24); +} + +/** + * @brief Get field s8_array from test_types message + * + * @return int8_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array) +{ + return MAVLINK_MSG_RETURN_int8_t_array(msg, s8_array, 3, 176); +} + +/** + * @brief Get field s16_array from test_types message + * + * @return int16_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array) +{ + return MAVLINK_MSG_RETURN_int16_t_array(msg, s16_array, 3, 154); +} + +/** + * @brief Get field s32_array from test_types message + * + * @return int32_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array) +{ + return MAVLINK_MSG_RETURN_int32_t_array(msg, s32_array, 3, 120); +} + +/** + * @brief Get field s64_array from test_types message + * + * @return int64_t_array + */ +static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array) +{ + return MAVLINK_MSG_RETURN_int64_t_array(msg, s64_array, 3, 48); +} + +/** + * @brief Get field f_array from test_types message + * + * @return float_array + */ +static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array) +{ + return MAVLINK_MSG_RETURN_float_array(msg, f_array, 3, 132); +} + +/** + * @brief Get field d_array from test_types message + * + * @return double_array + */ +static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array) +{ + return MAVLINK_MSG_RETURN_double_array(msg, d_array, 3, 72); +} + +/** + * @brief Decode a test_types message into a struct + * + * @param msg The message to decode + * @param test_types C-struct to decode the message contents into + */ +static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types) +{ +#if MAVLINK_NEED_BYTE_SWAP + test_types->u64 = mavlink_msg_test_types_get_u64(msg); + test_types->s64 = mavlink_msg_test_types_get_s64(msg); + test_types->d = mavlink_msg_test_types_get_d(msg); + mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array); + mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array); + mavlink_msg_test_types_get_d_array(msg, test_types->d_array); + test_types->u32 = mavlink_msg_test_types_get_u32(msg); + test_types->s32 = mavlink_msg_test_types_get_s32(msg); + test_types->f = mavlink_msg_test_types_get_f(msg); + mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array); + mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array); + mavlink_msg_test_types_get_f_array(msg, test_types->f_array); + test_types->u16 = mavlink_msg_test_types_get_u16(msg); + test_types->s16 = mavlink_msg_test_types_get_s16(msg); + mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array); + mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array); + test_types->c = mavlink_msg_test_types_get_c(msg); + mavlink_msg_test_types_get_s(msg, test_types->s); + test_types->u8 = mavlink_msg_test_types_get_u8(msg); + test_types->s8 = mavlink_msg_test_types_get_s8(msg); + mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array); + mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array); +#else + memcpy(test_types, MAVLINK_PAYLOAD(msg), 179); +#endif +} diff --git a/thirdParty/mavlink/include/test/test.h b/thirdParty/mavlink/include/test/test.h new file mode 100644 index 0000000000000000000000000000000000000000..700d291b5a70118cfb4cb3d7cb2999ae8eff5fd2 --- /dev/null +++ b/thirdParty/mavlink/include/test/test.h @@ -0,0 +1,53 @@ +/** @file + * @brief MAVLink comm protocol generated from test.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef TEST_H +#define TEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_TEST + + + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +// ENUM DEFINITIONS + + + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_test_types.h" + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // TEST_H diff --git a/thirdParty/mavlink/include/test/testsuite.h b/thirdParty/mavlink/include/test/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..3b72ad0093f0f75e1a9f3a1071313875a04da536 --- /dev/null +++ b/thirdParty/mavlink/include/test/testsuite.h @@ -0,0 +1,80 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from test.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef TEST_TESTSUITE_H +#define TEST_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_test(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + + mavlink_test_test(system_id, component_id); +} +#endif + + + + +static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_test_types_t packet2, packet1 = { + 93372036854775807ULL, + 93372036854776311LL, + 235.0, + { 93372036854777319, 93372036854777320, 93372036854777321 }, + { 93372036854778831, 93372036854778832, 93372036854778833 }, + { 627.0, 628.0, 629.0 }, + 963502456, + 963502664, + 745.0, + { 963503080, 963503081, 963503082 }, + { 963503704, 963503705, 963503706 }, + { 941.0, 942.0, 943.0 }, + 24723, + 24827, + { 24931, 24932, 24933 }, + { 25243, 25244, 25245 }, + 'E', + "FGHIJKLMN", + 198, + 9, + { 76, 77, 78 }, + { 21, 22, 23 }, + }; + if (sizeof(packet2) != 179) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_test_types_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); + mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ipayload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - 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] + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds) + put_float_by_index(msg, 8, accel_0); // b_f[0] + put_float_by_index(msg, 12, accel_1); // b_f[1] + put_float_by_index(msg, 16, accel_2); // b_f[2] + put_float_by_index(msg, 20, gyro_0); // b_f[0] + put_float_by_index(msg, 24, gyro_1); // b_f[1] + put_float_by_index(msg, 28, gyro_2); // b_f[2] - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 32, 34); } /** - * @brief Pack a nav_filter_bias message + * @brief Pack a nav_filter_bias message on a channel * @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 @@ -64,22 +78,56 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8 * @param gyro_2 b_f[2] * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - 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] + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds) + put_float_by_index(msg, 8, accel_0); // b_f[0] + put_float_by_index(msg, 12, accel_1); // b_f[1] + put_float_by_index(msg, 16, accel_2); // b_f[2] + put_float_by_index(msg, 20, gyro_0); // b_f[0] + put_float_by_index(msg, 24, gyro_1); // b_f[1] + put_float_by_index(msg, 28, gyro_2); // b_f[2] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 34); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a nav_filter_bias message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds) + * @param accel_0 b_f[0] + * @param accel_1 b_f[1] + * @param accel_2 b_f[2] + * @param gyro_0 b_f[0] + * @param gyro_1 b_f[1] + * @param gyro_2 b_f[2] + */ +static inline void mavlink_msg_nav_filter_bias_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds) + put_float_by_index(msg, 8, accel_0); // b_f[0] + put_float_by_index(msg, 12, accel_1); // b_f[1] + put_float_by_index(msg, 16, accel_2); // b_f[2] + put_float_by_index(msg, 20, gyro_0); // b_f[0] + put_float_by_index(msg, 24, gyro_1); // b_f[1] + put_float_by_index(msg, 28, gyro_2); // b_f[2] + + mavlink_finalize_message_chan_send(msg, chan, 32, 34); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a nav_filter_bias struct into a message * @@ -93,8 +141,6 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin return mavlink_msg_nav_filter_bias_pack(system_id, component_id, msg, nav_filter_bias->usec, nav_filter_bias->accel_0, nav_filter_bias->accel_1, nav_filter_bias->accel_2, nav_filter_bias->gyro_0, nav_filter_bias->gyro_1, nav_filter_bias->gyro_2); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a nav_filter_bias message * @param chan MAVLink channel to send the message @@ -107,41 +153,19 @@ 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 + 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_header_t hdr; - mavlink_nav_filter_bias_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN ) - payload.usec = usec; // uint64_t:Timestamp (microseconds) - payload.accel_0 = accel_0; // float:b_f[0] - payload.accel_1 = accel_1; // float:b_f[1] - payload.accel_2 = accel_2; // float:b_f[2] - payload.gyro_0 = gyro_0; // float:b_f[0] - payload.gyro_1 = gyro_1; // float:b_f[1] - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xFF, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 32); + mavlink_msg_nav_filter_bias_pack_chan_send(chan, msg, usec, accel_0, accel_1, accel_2, gyro_0, gyro_1, gyro_2); } #endif + // MESSAGE NAV_FILTER_BIAS UNPACKING + /** * @brief Get field usec from nav_filter_bias message * @@ -149,8 +173,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (uint64_t)(p->usec); + return MAVLINK_MSG_RETURN_uint64_t(msg, 0); } /** @@ -160,8 +183,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (float)(p->accel_0); + return MAVLINK_MSG_RETURN_float(msg, 8); } /** @@ -171,8 +193,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (float)(p->accel_1); + return MAVLINK_MSG_RETURN_float(msg, 12); } /** @@ -182,8 +203,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (float)(p->accel_2); + return MAVLINK_MSG_RETURN_float(msg, 16); } /** @@ -193,8 +213,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (float)(p->gyro_0); + return MAVLINK_MSG_RETURN_float(msg, 20); } /** @@ -204,8 +223,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (float)(p->gyro_1); + return MAVLINK_MSG_RETURN_float(msg, 24); } /** @@ -215,8 +233,7 @@ 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) { - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; - return (float)(p->gyro_2); + return MAVLINK_MSG_RETURN_float(msg, 28); } /** @@ -227,5 +244,15 @@ 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) { - memcpy( nav_filter_bias, msg->payload, sizeof(mavlink_nav_filter_bias_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(nav_filter_bias, MAVLINK_PAYLOAD(msg), 32); +#endif } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 10fe466da71cab186c6f10276f7115928d2d10bd..16d5b1d304cd45852684f242e50525c8ee6d3ef9 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -1,21 +1,20 @@ // 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 -#define MAVLINK_MSG_ID_RADIO_CALIBRATION_KEY 0x49 -#define MAVLINK_MSG_221_KEY 0x49 -typedef struct __mavlink_radio_calibration_t +typedef struct __mavlink_radio_calibration_t { - uint16_t aileron[3]; ///< Aileron setpoints: left, center, right - uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up - uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right - uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode - uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) - uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) - + uint16_t aileron[3]; ///< Aileron setpoints: left, center, right + uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up + uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right + uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode + uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) + uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) } mavlink_radio_calibration_t; + +#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42 +#define MAVLINK_MSG_ID_221_LEN 42 + #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 @@ -23,6 +22,19 @@ typedef struct __mavlink_radio_calibration_t #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 +#define MAVLINK_MESSAGE_INFO_RADIO_CALIBRATION { \ + "RADIO_CALIBRATION", \ + 6, \ + { { "aileron", MAVLINK_TYPE_UINT16_T, 3, 0, offsetof(mavlink_radio_calibration_t, aileron) }, \ + { "elevator", MAVLINK_TYPE_UINT16_T, 3, 6, offsetof(mavlink_radio_calibration_t, elevator) }, \ + { "rudder", MAVLINK_TYPE_UINT16_T, 3, 12, offsetof(mavlink_radio_calibration_t, rudder) }, \ + { "gyro", MAVLINK_TYPE_UINT16_T, 2, 18, offsetof(mavlink_radio_calibration_t, gyro) }, \ + { "pitch", MAVLINK_TYPE_UINT16_T, 5, 22, offsetof(mavlink_radio_calibration_t, pitch) }, \ + { "throttle", MAVLINK_TYPE_UINT16_T, 5, 32, offsetof(mavlink_radio_calibration_t, throttle) }, \ + } \ +} + + /** * @brief Pack a radio_calibration message * @param system_id ID of this system @@ -37,23 +49,23 @@ typedef struct __mavlink_radio_calibration_t * @param throttle Throttle curve setpoints (every 25%) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - 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%) + put_uint16_t_array_by_index(msg, 0, aileron, 3); // Aileron setpoints: left, center, right + put_uint16_t_array_by_index(msg, 6, elevator, 3); // Elevator setpoints: nose down, center, nose up + put_uint16_t_array_by_index(msg, 12, rudder, 3); // Rudder setpoints: nose left, center, nose right + put_uint16_t_array_by_index(msg, 18, gyro, 2); // Tail gyro mode/gain setpoints: heading hold, rate mode + put_uint16_t_array_by_index(msg, 22, pitch, 5); // Pitch curve setpoints (every 25%) + put_uint16_t_array_by_index(msg, 32, throttle, 5); // Throttle curve setpoints (every 25%) - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 42, 71); } /** - * @brief Pack a radio_calibration message + * @brief Pack a radio_calibration message on a channel * @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 @@ -66,21 +78,53 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin * @param throttle Throttle curve setpoints (every 25%) * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - 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%) + put_uint16_t_array_by_index(msg, 0, aileron, 3); // Aileron setpoints: left, center, right + put_uint16_t_array_by_index(msg, 6, elevator, 3); // Elevator setpoints: nose down, center, nose up + put_uint16_t_array_by_index(msg, 12, rudder, 3); // Rudder setpoints: nose left, center, nose right + put_uint16_t_array_by_index(msg, 18, gyro, 2); // Tail gyro mode/gain setpoints: heading hold, rate mode + put_uint16_t_array_by_index(msg, 22, pitch, 5); // Pitch curve setpoints (every 25%) + put_uint16_t_array_by_index(msg, 32, throttle, 5); // Throttle curve setpoints (every 25%) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 71); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a radio_calibration message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param aileron Aileron setpoints: left, center, right + * @param elevator Elevator setpoints: nose down, center, nose up + * @param rudder Rudder setpoints: nose left, center, nose right + * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode + * @param pitch Pitch curve setpoints (every 25%) + * @param throttle Throttle curve setpoints (every 25%) + */ +static inline void mavlink_msg_radio_calibration_pack_chan_send(mavlink_channel_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) +{ + msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + + put_uint16_t_array_by_index(msg, 0, aileron, 3); // Aileron setpoints: left, center, right + put_uint16_t_array_by_index(msg, 6, elevator, 3); // Elevator setpoints: nose down, center, nose up + put_uint16_t_array_by_index(msg, 12, rudder, 3); // Rudder setpoints: nose left, center, nose right + put_uint16_t_array_by_index(msg, 18, gyro, 2); // Tail gyro mode/gain setpoints: heading hold, rate mode + put_uint16_t_array_by_index(msg, 22, pitch, 5); // Pitch curve setpoints (every 25%) + put_uint16_t_array_by_index(msg, 32, throttle, 5); // Throttle curve setpoints (every 25%) + + mavlink_finalize_message_chan_send(msg, chan, 42, 71); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a radio_calibration struct into a message * @@ -94,8 +138,6 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u return mavlink_msg_radio_calibration_pack(system_id, component_id, msg, radio_calibration->aileron, radio_calibration->elevator, radio_calibration->rudder, radio_calibration->gyro, radio_calibration->pitch, radio_calibration->throttle); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a radio_calibration message * @param chan MAVLink channel to send the message @@ -107,51 +149,27 @@ 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%) */ -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) +#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_header_t hdr; - mavlink_radio_calibration_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN ) - memcpy(payload.aileron, aileron, sizeof(payload.aileron)); // uint16_t[3]:Aileron setpoints: left, center, right - memcpy(payload.elevator, elevator, sizeof(payload.elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up - memcpy(payload.rudder, rudder, sizeof(payload.rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right - memcpy(payload.gyro, gyro, sizeof(payload.gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode - memcpy(payload.pitch, pitch, sizeof(payload.pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) - memcpy(payload.throttle, throttle, sizeof(payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0x49, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 42); + mavlink_msg_radio_calibration_pack_chan_send(chan, msg, aileron, elevator, rudder, gyro, pitch, throttle); } #endif + // MESSAGE RADIO_CALIBRATION UNPACKING + /** * @brief Get field aileron from radio_calibration message * * @return Aileron setpoints: left, center, right */ -static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* aileron) +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(aileron, p->aileron, sizeof(p->aileron)); - return sizeof(p->aileron); + return MAVLINK_MSG_RETURN_uint16_t_array(msg, aileron, 3, 0); } /** @@ -159,12 +177,9 @@ 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* elevator) +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(elevator, p->elevator, sizeof(p->elevator)); - return sizeof(p->elevator); + return MAVLINK_MSG_RETURN_uint16_t_array(msg, elevator, 3, 6); } /** @@ -172,12 +187,9 @@ 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* rudder) +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(rudder, p->rudder, sizeof(p->rudder)); - return sizeof(p->rudder); + return MAVLINK_MSG_RETURN_uint16_t_array(msg, rudder, 3, 12); } /** @@ -185,12 +197,9 @@ 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* gyro) +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(gyro, p->gyro, sizeof(p->gyro)); - return sizeof(p->gyro); + return MAVLINK_MSG_RETURN_uint16_t_array(msg, gyro, 2, 18); } /** @@ -198,12 +207,9 @@ 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* pitch) +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(pitch, p->pitch, sizeof(p->pitch)); - return sizeof(p->pitch); + return MAVLINK_MSG_RETURN_uint16_t_array(msg, pitch, 5, 22); } /** @@ -211,12 +217,9 @@ 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* throttle) +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(throttle, p->throttle, sizeof(p->throttle)); - return sizeof(p->throttle); + return MAVLINK_MSG_RETURN_uint16_t_array(msg, throttle, 5, 32); } /** @@ -227,5 +230,14 @@ 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) { - memcpy( radio_calibration, msg->payload, sizeof(mavlink_radio_calibration_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(radio_calibration, MAVLINK_PAYLOAD(msg), 42); +#endif } 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 e346fdf1a8d70c54ab01d624a856c5b4026a11ea..8dca0f90eaf1baaed482041e40dc2fbfdf202324 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -1,19 +1,29 @@ // 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 -#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_KEY 0xEF -#define MAVLINK_MSG_222_KEY 0xEF -typedef struct __mavlink_ualberta_sys_status_t +typedef struct __mavlink_ualberta_sys_status_t { - uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM - uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM - uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE - + uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM + uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM + uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE } mavlink_ualberta_sys_status_t; +#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN 3 +#define MAVLINK_MSG_ID_222_LEN 3 + + + +#define MAVLINK_MESSAGE_INFO_UALBERTA_SYS_STATUS { \ + "UALBERTA_SYS_STATUS", \ + 3, \ + { { "mode", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_ualberta_sys_status_t, mode) }, \ + { "nav_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_ualberta_sys_status_t, nav_mode) }, \ + { "pilot", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_ualberta_sys_status_t, pilot) }, \ + } \ +} + + /** * @brief Pack a ualberta_sys_status message * @param system_id ID of this system @@ -25,20 +35,20 @@ typedef struct __mavlink_ualberta_sys_status_t * @param pilot Pilot mode, see UALBERTA_PILOT_MODE * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - 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 + put_uint8_t_by_index(msg, 0, mode); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM + put_uint8_t_by_index(msg, 1, nav_mode); // Navigation mode, see UALBERTA_NAV_MODE ENUM + put_uint8_t_by_index(msg, 2, pilot); // Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); + return mavlink_finalize_message(msg, system_id, component_id, 3, 15); } /** - * @brief Pack a ualberta_sys_status message + * @brief Pack a ualberta_sys_status message on a channel * @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 @@ -48,18 +58,44 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, u * @param pilot Pilot mode, see UALBERTA_PILOT_MODE * @return length of the message in bytes (excluding serial stream start sign) */ -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) +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) { - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - 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 + put_uint8_t_by_index(msg, 0, mode); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM + put_uint8_t_by_index(msg, 1, nav_mode); // Navigation mode, see UALBERTA_NAV_MODE ENUM + put_uint8_t_by_index(msg, 2, pilot); // Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 15); } +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a ualberta_sys_status message on a channel and send + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM + * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM + * @param pilot Pilot mode, see UALBERTA_PILOT_MODE + */ +static inline void mavlink_msg_ualberta_sys_status_pack_chan_send(mavlink_channel_t chan, + mavlink_message_t* msg, + uint8_t mode,uint8_t nav_mode,uint8_t pilot) +{ + msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + + put_uint8_t_by_index(msg, 0, mode); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM + put_uint8_t_by_index(msg, 1, nav_mode); // Navigation mode, see UALBERTA_NAV_MODE ENUM + put_uint8_t_by_index(msg, 2, pilot); // Pilot mode, see UALBERTA_PILOT_MODE + + mavlink_finalize_message_chan_send(msg, chan, 3, 15); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + + /** * @brief Encode a ualberta_sys_status struct into a message * @@ -73,8 +109,6 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, return mavlink_msg_ualberta_sys_status_pack(system_id, component_id, msg, ualberta_sys_status->mode, ualberta_sys_status->nav_mode, ualberta_sys_status->pilot); } - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a ualberta_sys_status message * @param chan MAVLink channel to send the message @@ -83,37 +117,19 @@ 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 + static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - mavlink_header_t hdr; - mavlink_ualberta_sys_status_t payload; - - MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN ) - payload.mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM - payload.nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM - payload.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 ); - mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) ); - - crc_init(&hdr.ck); - crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); - crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); - crc_accumulate( 0xEF, &hdr.ck); /// include key in X25 checksum - mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); - MAVLINK_BUFFER_CHECK_END + MAVLINK_ALIGNED_MESSAGE(msg, 3); + mavlink_msg_ualberta_sys_status_pack_chan_send(chan, msg, mode, nav_mode, pilot); } #endif + // MESSAGE UALBERTA_SYS_STATUS UNPACKING + /** * @brief Get field mode from ualberta_sys_status message * @@ -121,8 +137,7 @@ 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) { - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; - return (uint8_t)(p->mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 0); } /** @@ -132,8 +147,7 @@ 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) { - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; - return (uint8_t)(p->nav_mode); + return MAVLINK_MSG_RETURN_uint8_t(msg, 1); } /** @@ -143,8 +157,7 @@ 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) { - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; - return (uint8_t)(p->pilot); + return MAVLINK_MSG_RETURN_uint8_t(msg, 2); } /** @@ -155,5 +168,11 @@ 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) { - memcpy( ualberta_sys_status, msg->payload, sizeof(mavlink_ualberta_sys_status_t)); +#if MAVLINK_NEED_BYTE_SWAP + 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); +#else + memcpy(ualberta_sys_status, MAVLINK_PAYLOAD(msg), 3); +#endif } diff --git a/thirdParty/mavlink/include/ualberta/testsuite.h b/thirdParty/mavlink/include/ualberta/testsuite.h new file mode 100644 index 0000000000000000000000000000000000000000..8cf86d30b236421d2f70cf574fe81391d998cdac --- /dev/null +++ b/thirdParty/mavlink/include/ualberta/testsuite.h @@ -0,0 +1,122 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from ualberta.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef UALBERTA_TESTSUITE_H +#define UALBERTA_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_common(uint8_t, uint8_t); +static void mavlink_test_ualberta(uint8_t, uint8_t); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +{ + mavlink_test_common(system_id, component_id); + mavlink_test_ualberta(system_id, component_id); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_nav_filter_bias(uint8_t system_id, uint8_t component_id) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_nav_filter_bias_t packet2, packet1 = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + }; + if (sizeof(packet2) != 32) { + packet2 = packet1; // cope with alignment within the packet + } + mavlink_msg_nav_filter_bias_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_nav_filter_bias_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + mavlink_msg_nav_filter_bias_pack(system_id, component_id, &msg , packet1.usec , packet1.accel_0 , packet1.accel_1 , packet1.accel_2 , packet1.gyro_0 , packet1.gyro_1 , packet1.gyro_2 ); + mavlink_msg_nav_filter_bias_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.accel_0 , packet1.accel_1 , packet1.accel_2 , packet1.gyro_0 , packet1.gyro_1 , packet1.gyro_2 ); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i 3 + + Valid data types for parameter interface + + IEEE-754 floating point number + + + 32 bit unsigned integer (C99: uint32_t) + + + 32 bit signed integer (C99: int32_t) + + + Micro air vehicle / autopilot classes. This identifies the individual model. @@ -56,78 +81,114 @@ - - System is not ready to fly, booting, calibrating, etc. - - - System is allowed to be active, under assisted RC control. Level of stabilization depends on MAV_FLIGTH_MODE + + System is currently on ground. - + System is allowed to be active, under manual (RC) control, no stabilization - + System is allowed to be active, under autonomous control, manual setpoint - - System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) + + System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. + + + The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. + + + RC control with stabilization; let go of the sticks and it will level. + + + The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. + + + Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. + + + Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. + + + Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_MODE_AUTO_VECTOR with user input) + + + Custom test mode, depends on individual autopilot. + + + Custom test mode, depends on individual autopilot. - - UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. + + Custom test mode, depends on individual autopilot. - - - System is currently on ground. + + + System is currently on ground. Calibrating acceleraometers. + + + System is currently on ground. Calibrating Gyros. + + + System is currently on ground. Calibrating magnetometer. - + + System is currently on ground. Calibrating pressuresensors. + + + System is currently on ground. Waiting for GPS Lock. + + + System is currently on ground and ready. + + No interaction of the autopilot with the actuator outputs, pure manual flight. - + System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. - + System is holding its current position and disabled the mission management. Loitering in mission mode is NOT the hold type, but still mission mode. - + System is navigating towards the next waypoint and following a mission script. - - System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_FLIGHT_MODE_FBW_CURSOR_CONTROL + + System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_MODE_FBW_CURSOR_CONTROL - + System is returning straight to home position. Once it reaches there it will hover or loiter at the autopilot's default holding settings. - + System is landing. Throttle is controlled by the autopilot. After getting closer than 30 meters, the course will lock to the current heading. Flare, throttle, flaps, gear, and other events can be scripted based on distance to landing point. - + System lost its position input and is in attitude / flight stabilization only. - + The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. - + RC control with stabilization; let go of the sticks and it will level. - + The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. - + Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. - + Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. - - Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_FLIGHT_MODE_AUTO_VECTOR with user input) + + Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_MODE_AUTO_VECTOR with user input) - + Custom test mode, depends on individual autopilot. - + Custom test mode, depends on individual autopilot. - + Custom test mode, depends on individual autopilot. @@ -147,6 +208,9 @@ System is active and might be already airborne. Motors are engaged. + + System is active and might be already airborne. Motors are engaged. + System is in a non-normal flight mode. It can however still navigate. @@ -173,10 +237,10 @@ Normal helicopter with tail rotor. - + Ground installation - + Operator control unit / ground control station @@ -188,13 +252,13 @@ Rocket - + Ground rover - + Surface vessel, boat, ship - + Submarine @@ -203,6 +267,12 @@ Octorotor + + Octorotor + + + Flapping wing + @@ -480,7 +550,7 @@ Set system mode. Mode, as defined by ENUM MAV_MODE - Empty + Flight Mode, as defined by ENUM MAV_FLIGHT_MODE Empty Empty Empty @@ -669,7 +739,7 @@ Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) Autopilot type / class. defined in MAV_CLASS ENUM System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - Navigation mode, see MAV_FLIGHT_MODE ENUM + Navigation mode, see MAV_MODE ENUM System status flag, see MAV_STATUS ENUM System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN @@ -678,18 +748,19 @@ 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. - Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 10'000) should be always below 10'000 Battery voltage, in millivolts (1 = 1 millivolt) - Battery current, in milliamperes (1 = 1 milliampere) + Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current Watts consumed from this battery since startup - Remaining battery energy: (0%: 0, 100%: 255) - Dropped packets on all links (packets that were corrupted on reception on the MAV) - Dropped packets on all links (packets that were corrupted on reception) - Dropped packets on all links (packets that were corrupted on reception) - Dropped packets on all links (packets that were corrupted on reception) + Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + Autopilot-specific errors + Autopilot-specific errors + Autopilot-specific errors + Autopilot-specific errors @@ -732,11 +803,6 @@ The system setting the mode The new mode - - Set the system navigation mode, as defined by enum MAV_FLIGHT_MODE. The navigation mode applies to the whole aircraft and thus all components. - The system setting the mode - The new navigation mode - Set the system safety mode, as defined by enum MAV_SAFETY. The navigation mode applies to the whole aircraft and thus all components. The system setting the mode @@ -782,7 +848,7 @@ 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 + Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 Number of satellites visible. If unknown, set to 255 @@ -877,11 +943,12 @@ GPS HDOP GPS VDOP GPS ground speed - Compass heading in degrees, 0..360 degrees + Course over ground (NOT heading, but direction of movement) in degrees, 0.0..359.99 degrees. If unknown, set to: -1 Number of satellites visible 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. + Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. RC channel 1 value, in microseconds RC channel 2 value, in microseconds RC channel 3 value, in microseconds @@ -894,6 +961,7 @@ The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 + Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 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 @@ -906,6 +974,7 @@ 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 port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. Servo output 1 value, in microseconds Servo output 2 value, in microseconds Servo output 3 value, in microseconds @@ -915,7 +984,7 @@ 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 @@ -934,43 +1003,43 @@ 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 @@ -1013,6 +1082,13 @@ WGS84 Altitude in meters * 1000 (positive for up) Desired yaw angle in degrees * 100 + + Set the current global position setpoint. + WGS84 Latitude position in degrees * 1E7 + WGS84 Longitude position in degrees * 1E7 + WGS84 Altitude in meters * 1000 (positive for up) + Desired yaw angle in degrees * 100 + 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 diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index 0eb3bb638e85b1098cd16207e00f537fdc5b7c22..a9a51d698944431715810ead12b691db74d636af 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -10,18 +10,7 @@ - - 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 @@ -29,7 +18,7 @@ Exposure time, in microseconds Camera gain - + Timestamp IMU seq Roll angle in rad @@ -43,10 +32,10 @@ Ground truth Y Ground truth Z - + 0 to disable, 1 to enable - + Camera id Camera # (starts with 0) Timestamp @@ -71,7 +60,7 @@ Ground truth Y Ground truth Z - + Timestamp (milliseconds) Global X position Global Y position @@ -80,7 +69,7 @@ Pitch angle in rad Yaw angle in rad - + Timestamp (milliseconds) Global X position Global Y position @@ -89,13 +78,13 @@ 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 @@ -105,7 +94,7 @@ 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 @@ -115,14 +104,14 @@ 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 @@ -131,7 +120,7 @@ pitch orientation yaw orientation - + ADC1 (J405 ADC3, LPC2148 AD0.6) ADC2 (J405 ADC5, LPC2148 AD0.2) ADC3 (J405 ADC6, LPC2148 AD0.1) @@ -140,26 +129,18 @@ 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 @@ -167,19 +148,19 @@ 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. @@ -193,7 +174,7 @@ 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. @@ -210,18 +191,18 @@ 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 @@ -230,6 +211,17 @@ Orientation Descriptor Harris operator response at this location + + + 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 diff --git a/thirdParty/mavlink/missionlib/mavlink_parameters.c b/thirdParty/mavlink/missionlib/mavlink_parameters.c index 420b296f86a0456d6a55d2fae2e6903e6751350e..8cd911e8ab8e627e99d7c96f0771c31187ecaecd 100644 --- a/thirdParty/mavlink/missionlib/mavlink_parameters.c +++ b/thirdParty/mavlink/missionlib/mavlink_parameters.c @@ -86,6 +86,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess &tx_msg, pm.param_names[i], pm.param_values[i], + 0, pm.size, i); mavlink_missionlib_send_message(&tx_msg); @@ -93,6 +94,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess mavlink_msg_param_value_send(MAVLINK_COMM_0, pm.param_names[i], pm.param_values[i], + 0, pm.size, i); #endif @@ -131,6 +133,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess &tx_msg, pm.param_names[pm.next_param], pm.param_values[pm.next_param], + 0, pm.size, pm.next_param); mavlink_missionlib_send_message(&tx_msg); @@ -138,6 +141,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess mavlink_msg_param_value_send(MAVLINK_COMM_0, pm.param_names[pm.next_param], pm.param_values[pm.next_param], + 0, pm.size, pm.next_param); #endif diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c index 96e575173db9238a8be142ae8f3c4b0e7274928f..eb75d4b7cd5b9eda93c4e8b10387b7411136639f 100644 --- a/thirdParty/mavlink/missionlib/testing/main.c +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -42,6 +42,7 @@ #include #include #include +#include #if (defined __QNX__) | (defined __QNXNTO__) /* QNX specific headers */ #include @@ -169,6 +170,12 @@ uint64_t mavlink_missionlib_get_system_timestamp() return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; } +float roll, pitch, yaw; +uint32_t latitude, longitude, altitude; +uint16_t speedx, speedy, speedz; +float rollspeed, pitchspeed, yawspeed; +bool hilEnabled = false; + int main(int argc, char* argv[]) { // Initialize MAVLink @@ -238,13 +245,13 @@ int main(int argc, char* argv[]) { bytes_sent = 0; - /*Send Heartbeat */ - mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); + /* Send Heartbeat */ + mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0, 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 Status */ - mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); + mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); @@ -255,17 +262,25 @@ int main(int argc, char* argv[]) len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + /* Send global position */ + if (hilEnabled) + { + mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, latitude, longitude, altitude, speedx, speedy, speedz, (yaw/M_PI)*180*100); + 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); + mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send HIL outputs */ float roll_ailerons = 0; // -1 .. 1 - float pitch_elevator = -0.2; // -1 .. 1 + float pitch_elevator = 0.2; // -1 .. 1 float yaw_rudder = 0.1; // -1 .. 1 float throttle = 0.9; // 0 .. 1 - mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode); + mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); @@ -300,6 +315,19 @@ int main(int argc, char* argv[]) mavlink_msg_hil_state_decode(&msg, &hil); printf("Received HIL state:\n"); printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw); + roll = hil.roll; + pitch = hil.pitch; + yaw = hil.yaw; + rollspeed = hil.rollspeed; + pitchspeed = hil.pitchspeed; + yawspeed = hil.yawspeed; + speedx = hil.vx; + speedy = hil.vy; + speedz = hil.vz; + latitude = hil.lat; + longitude = hil.lon; + altitude = hil.alt; + hilEnabled = true; } } }