From cd2bcb0749a28367a225d4b709c20213ebc51e6c Mon Sep 17 00:00:00 2001 From: LM Date: Wed, 31 Aug 2011 13:06:43 +0200 Subject: [PATCH 001/131] Tested and validated parameter transmission --- src/uas/UAS.cc | 36 ++++-- src/ui/QGCParamWidget.cc | 234 ++++++++++++++++++++++++++++++--------- 2 files changed, 211 insertions(+), 59 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 0a6735341f..8673166738 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -664,6 +664,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) int component = message.compid; mavlink_param_union_t val; val.param_float = value.param_value; + val.type = value.param_type; // Convert to machine order if necessary //#if MAVLINK_NEED_BYTE_SWAP @@ -686,22 +687,37 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) switch (value.param_type) { case MAVLINK_TYPE_FLOAT: - parameters.value(component)->insert(parameterName, val.param_float); + { + // Variant + QVariant param(val.param_float); + parameters.value(component)->insert(parameterName, param); // Emit change - emit parameterChanged(uasId, message.compid, parameterName, val.param_float); - emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val.param_float); + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + qDebug() << "RECEIVED PARAM:" << param; + } break; case MAVLINK_TYPE_UINT32_T: - parameters.value(component)->insert(parameterName, val.param_uint32); + { + // Variant + QVariant param(val.param_uint32); + parameters.value(component)->insert(parameterName, param); // Emit change - emit parameterChanged(uasId, message.compid, parameterName, val.param_uint32); - emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val.param_uint32); + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + qDebug() << "RECEIVED PARAM:" << param; + } break; case MAVLINK_TYPE_INT32_T: - parameters.value(component)->insert(parameterName, val.param_int32); + { + // Variant + QVariant param(val.param_int32); + parameters.value(component)->insert(parameterName, param); // Emit change - emit parameterChanged(uasId, message.compid, parameterName, val.param_int32); - emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val.param_int32); + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + qDebug() << "RECEIVED PARAM:" << param; + } break; default: qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << value.param_type; @@ -1861,6 +1877,8 @@ void UAS::setParameter(const int component, const QString& id, const QVariant& v p.target_system = (uint8_t)uasId; p.target_component = (uint8_t)component; + qDebug() << "SENT PARAM:" << value; + // Copy string into buffer, ensuring not to exceed the buffer size for (unsigned int i = 0; i < sizeof(p.param_id); i++) { diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index f7f1b25477..e167629fc9 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include "QGCParamWidget.h" #include "UASInterface.h" @@ -291,8 +292,9 @@ void QGCParamWidget::addComponent(int uas, int component, QString componentName) Q_UNUSED(uas); if (components->contains(component)) { // Update existing - components->value(component)->setData(0, Qt::DisplayRole, componentName); - components->value(component)->setData(1, Qt::DisplayRole, QString::number(component)); + components->value(component)->setData(0, Qt::DisplayRole, QString("%1 (#%2)").arg(componentName).arg(component)); + //components->value(component)->setData(1, Qt::DisplayRole, QString::number(component)); + components->value(component)->setFirstColumnSpanned(true); } else { // Add new QStringList list(QString("%1 (#%2)").arg(componentName).arg(component)); @@ -451,6 +453,7 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa */ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, QVariant value) { + qDebug() << "PARAM WIDGET GOT PARAM:" << value; Q_UNUSED(uas); // Reference to item in tree QTreeWidgetItem* parameterItem = NULL; @@ -458,20 +461,20 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, // Get component if (!components->contains(component)) { - QString componentName; - switch (component) - { - case MAV_COMP_ID_CAMERA: - componentName = tr("Camera (#%1)").arg(component); - break; - case MAV_COMP_ID_IMU: - componentName = tr("IMU (#%1)").arg(component); - break; - default: - componentName = tr("Component #").arg(component); - break; - } - +// QString componentName; +// switch (component) +// { +// case MAV_COMP_ID_CAMERA: +// componentName = tr("Camera (#%1)").arg(component); +// break; +// case MAV_COMP_ID_IMU: +// componentName = tr("IMU (#%1)").arg(component); +// break; +// default: +// componentName = tr("Component #").arg(component); +// break; +// } + QString componentName = tr("Component #").arg(component); addComponent(uas, component, componentName); } @@ -518,10 +521,10 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, // Insert parameter into map QStringList plist; plist.append(parameterName); - plist.append(QString::number(value.toDouble())); // CREATE PARAMETER ITEM parameterItem = new QTreeWidgetItem(plist); // CONFIGURE PARAMETER ITEM + parameterItem->setData(1, Qt::DisplayRole, value); compParamGroups->value(parent)->addChild(parameterItem); parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable); @@ -549,10 +552,10 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, // Insert parameter into map QStringList plist; plist.append(parameterName); - plist.append(QString::number(value.toDouble())); // CREATE PARAMETER ITEM parameterItem = new QTreeWidgetItem(plist); // CONFIGURE PARAMETER ITEM + parameterItem->setData(1, Qt::DisplayRole, value); components->value(component)->addChild(parameterItem); parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable); @@ -563,8 +566,18 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, parameterItem->setBackground(0, QBrush(QColor(0, 0, 0))); parameterItem->setBackground(1, Qt::NoBrush); // Add tooltip - parameterItem->setToolTip(0, paramToolTips.value(parameterName, "")); - parameterItem->setToolTip(1, paramToolTips.value(parameterName, "")); + QString tooltipFormat; + if (paramDefault.contains(parameterName)) + { + tooltipFormat = tr("Default: %1, %2"); + tooltipFormat = tooltipFormat.arg(paramToolTips.value(parameterName, ""), paramDefault.value(parameterName)); + } + else + { + tooltipFormat = paramToolTips.value(parameterName, ""); + } + parameterItem->setToolTip(0, tooltipFormat); + parameterItem->setToolTip(1, tooltipFormat); //tree->update(); if (changedValues.contains(component)) changedValues.value(component)->remove(parameterName); @@ -619,28 +632,45 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) } QMap* map = changedValues.value(key, NULL); if (map) { - bool ok; QString str = current->data(0, Qt::DisplayRole).toString(); - float value = current->data(1, Qt::DisplayRole).toDouble(&ok); + QVariant value = current->data(1, Qt::DisplayRole); + qDebug() << "CHANGED PARAM:" << value; // Set parameter on changed list to be transmitted to MAV - if (ok) { - if (ok) { - statusLabel->setText(tr("Changed Param %1:%2: %3").arg(key).arg(str).arg(value)); - //qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; - // Changed values list - if (map->contains(str)) map->remove(str); - map->insert(str, value); - - // Check if the value was numerically changed - if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, 0.0f) != value) { - current->setBackground(0, QBrush(QColor(QGC::colorOrange))); - current->setBackground(1, QBrush(QColor(QGC::colorOrange))); - } + statusLabel->setText(tr("Changed Param %1:%2: %3").arg(key).arg(str).arg(value.toDouble())); + //qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; + // Changed values list + if (map->contains(str)) map->remove(str); + map->insert(str, value); + + // Check if the value was numerically changed + if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, value.toDouble()-1) != value) { + current->setBackground(0, QBrush(QColor(QGC::colorOrange))); + current->setBackground(1, QBrush(QColor(QGC::colorOrange))); + } - // All parameters list - if (parameters.value(key)->contains(str)) parameters.value(key)->remove(str); - parameters.value(key)->insert(str, value); + switch (parameters.value(key)->value(str).type()) + { + case QVariant::Int: + { + QVariant fixedValue(value.toInt()); + parameters.value(key)->insert(str, fixedValue); + } + break; + case QVariant::UInt: + { + QVariant fixedValue(value.toUInt()); + parameters.value(key)->insert(str, fixedValue); + } + break; + case QMetaType::Float: + { + QVariant fixedValue(value.toFloat()); + parameters.value(key)->insert(str, fixedValue); } + break; + default: + qCritical() << "ABORTED PARAM UPDATE, NO VALID QVARIANT TYPE"; + return; } } } @@ -672,8 +702,26 @@ void QGCParamWidget::saveParameters() for (j = comp->begin(); j != comp->end(); ++j) { QString paramValue("%1"); - paramValue = paramValue.arg(j.value().toDouble(), 25, 'g', 12); - in << mav->getUASID() << "\t" << compid << "\t" << j.key() << "\t" << paramValue << "\n"; + QString paramType("%1"); + switch (j.value().type()) + { + case QVariant::Int: + paramValue = paramValue.arg(j.value().toInt()); + paramType.arg(MAVLINK_TYPE_INT32_T); + break; + case QVariant::UInt: + paramValue = paramValue.arg(j.value().toUInt()); + paramType.arg(MAVLINK_TYPE_UINT32_T); + break; + case QMetaType::Float: + paramValue = paramValue.arg(j.value().toDouble(), 25, 'g', 12); + paramType.arg(MAVLINK_TYPE_FLOAT); + break; + default: + qCritical() << "ABORTED PARAM WRITE TO FILE, NO VALID QVARIANT TYPE" << j.value(); + return; + } + in << mav->getUASID() << "\t" << compid << "\t" << j.key() << "\t" << paramValue << "\t" << paramType << "\n"; in.flush(); } } @@ -697,14 +745,14 @@ void QGCParamWidget::loadParameters() QString line = in.readLine(); if (!line.startsWith("#")) { QStringList wpParams = line.split("\t"); - if (wpParams.size() == 4) { + if (wpParams.size() == 5) { // Only load parameters for right mav if (mav->getUASID() == wpParams.at(0).toInt()) { bool changed = false; int component = wpParams.at(1).toInt(); QString parameterName = wpParams.at(2); - if (!parameters.contains(component) || parameters.value(component)->value(parameterName, 0.0f) != (float)wpParams.at(3).toDouble()) { + if (!parameters.contains(component) || parameters.value(component)->value(parameterName, wpParams.at(3).toDouble()-3.0f) != (float)wpParams.at(3).toDouble()) { changed = true; } @@ -722,9 +770,20 @@ void QGCParamWidget::loadParameters() changedValues.value(wpParams.at(1).toInt())->remove(wpParams.at(2)); } - changedValues.value(wpParams.at(1).toInt())->insert(wpParams.at(2), (float)wpParams.at(3).toDouble()); + switch (wpParams.at(3).toUInt()) + { + case MAVLINK_TYPE_FLOAT: + changedValues.value(wpParams.at(1).toInt())->insert(wpParams.at(2), wpParams.at(3).toFloat()); + break; + case MAVLINK_TYPE_UINT32_T: + changedValues.value(wpParams.at(1).toInt())->insert(wpParams.at(2), wpParams.at(3).toUInt()); + break; + case MAVLINK_TYPE_INT32_T: + changedValues.value(wpParams.at(1).toInt())->insert(wpParams.at(2), wpParams.at(3).toInt()); + break; + } - //qDebug() << "MARKING COMP" << wpParams.at(1).toInt() << "PARAM" << wpParams.at(2) << "VALUE" << (float)wpParams.at(3).toDouble() << "AS CHANGED"; + qDebug() << "MARKING COMP" << wpParams.at(1).toInt() << "PARAM" << wpParams.at(2) << "VALUE" << (float)wpParams.at(3).toDouble() << "AS CHANGED"; // Mark in UI @@ -817,7 +876,31 @@ void QGCParamWidget::retransmissionGuardTick() foreach (QString key, missingParams->keys()) { if (count < retransmissionBurstRequestSize) { // Re-request write operation - emit parameterChanged(component, key, missingParams->value(key)); + QVariant value = missingParams->value(key); + switch (parameters.value(component)->value(key).type()) + { + case QVariant::Int: + { + QVariant fixedValue(value.toInt()); + emit parameterChanged(component, key, fixedValue); + } + break; + case QVariant::UInt: + { + QVariant fixedValue(value.toUInt()); + emit parameterChanged(component, key, fixedValue); + } + break; + case QMetaType::Float: + { + QVariant fixedValue(value.toFloat()); + emit parameterChanged(component, key, fixedValue); + } + break; + default: + qCritical() << "ABORTED PARAM RETRANSMISSION, NO VALID QVARIANT TYPE"; + return; + } statusLabel->setText(tr("Requested rewrite of: %1: %2").arg(key).arg(missingParams->value(key).toDouble())); count++; } else { @@ -858,7 +941,35 @@ void QGCParamWidget::setParameter(int component, QString parameterName, QVariant statusLabel->setText(tr("REJ. %1 > max").arg(value.toDouble())); return; } - emit parameterChanged(component, parameterName, value); + + switch (parameters.value(component)->value(parameterName).type()) + { + case QVariant::Int: + { + QVariant fixedValue(value.toInt()); + emit parameterChanged(component, parameterName, fixedValue); + qDebug() << "PARAM WIDGET SENT:" << fixedValue; + } + break; + case QVariant::UInt: + { + QVariant fixedValue(value.toUInt()); + emit parameterChanged(component, parameterName, fixedValue); + qDebug() << "PARAM WIDGET SENT:" << fixedValue; + } + break; + case QMetaType::Float: + { + QVariant fixedValue(value.toFloat()); + emit parameterChanged(component, parameterName, fixedValue); + qDebug() << "PARAM WIDGET SENT:" << fixedValue; + } + break; + default: + qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; + return; + } + // Wait for parameter to be written back // mark it therefore as missing if (!transmissionMissingWriteAckPackets.contains(component)) @@ -914,8 +1025,6 @@ void QGCParamWidget::setParameters() // Enable guard setRetransmissionGuardEnabled(true); } - - changedValues.clear(); } /** @@ -924,8 +1033,33 @@ void QGCParamWidget::setParameters() */ void QGCParamWidget::writeParameters() { - if (!mav) return; - mav->writeParametersToStorage(); + int changedParamCount = 0; + + QMap*>::iterator i; + for (i = changedValues.begin(); i != changedValues.end(); ++i) + { + // Iterate through the parameters of the component + QMap* comp = i.value(); + { + QMap::iterator j; + for (j = comp->begin(); j != comp->end(); ++j) + { + changedParamCount++; + } + } + } + + if (changedParamCount > 0) + { + QMessageBox msgBox; + msgBox.setText(tr("There are locally changed parameters. Please transmit them first () or update them with the onboard values () before storing onboard from RAM to ROM.")); + msgBox.exec(); + } + else + { + if (!mav) return; + mav->writeParametersToStorage(); + } } void QGCParamWidget::readParameters() -- GitLab From 7fe7ced34ebe8718e6f52e7985c74d8256f23804 Mon Sep 17 00:00:00 2001 From: LM Date: Wed, 31 Aug 2011 13:51:33 +0200 Subject: [PATCH 002/131] Minor fixed to simulation --- src/comm/QGCFlightGearLink.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index a5eabbf873..00d77e2e50 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -361,10 +361,9 @@ fgRoot = "--fg-root=/usr/share/flightgear/data"; processCall << fgRoot; processCall << fgScenery; -processCall << "--generic=socket,out,50,127.0.0.1,49005,udp,ardupilot"; -processCall << "--generic=socket,in,50,127.0.0.1,49000,udp,ardupilot"; +processCall << "--generic=socket,out,50,127.0.0.1,49005,udp,qgroundcontrol"; +processCall << "--generic=socket,in,50,127.0.0.1,49000,udp,qgroundcontrol"; processCall << "--in-air"; -processCall << "--altitude=10"; processCall << "--vc=90"; processCall << "--heading=300"; processCall << "--timeofday=noon"; @@ -386,8 +385,9 @@ if (mav->getSystemType() == MAV_TYPE_QUADROTOR) processCall << "--prop:/engines/engine[2]/running=true"; processCall << "--prop:/engines/engine[3]/running=true"; } -processCall << QString("--lat=%1").arg(mav->getLatitude()); -processCall << QString("--lon=%1").arg(mav->getLongitude()); +processCall << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude()); +processCall << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude()); +processCall << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude()); // Add new argument with this: processCall << ""; processCall << QString("--aircraft=%2").arg(aircraft); -- GitLab From a98252c5ccb92621f957a226420ecc4df20f9fba Mon Sep 17 00:00:00 2001 From: lm Date: Wed, 31 Aug 2011 14:27:21 +0200 Subject: [PATCH 003/131] Minor compile time fixes. --- src/comm/QGCFlightGearLink.cc | 4 ++-- src/ui/map3D/QGCGoogleEarthView.cc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index a5eabbf873..95c8f130f3 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -350,8 +350,8 @@ fgScenery = "--fg-scenery=/Applications/FlightGear.app/Contents/Resources/data/S #endif #ifdef Q_OS_WIN32 -processFgfs = "C:\Program Files (x86)\FlightGear\bin\Win32\fgfs"; -fgRoot = "--fg-root=C:\Program Files (x86)\FlightGear\data"; +processFgfs = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs"; +fgRoot = "--fg-root=C:\\Program Files (x86)\\FlightGear\\data"; #endif #ifdef Q_OS_LINUX diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index fe0d9928d4..2c16bb010a 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -461,7 +461,7 @@ QVariant QGCGoogleEarthView::documentElement(QString name) name.prepend("JScript_"); HRESULT res = doc->getElementById(QStringToBSTR(name), &element); //BSTR elemString; - if (element) { + if (SUCCEEDED(res) && element) { //element->get_innerHTML(&elemString); VARIANT var; var.vt = VT_BSTR; -- GitLab From 51069e0618f86031ca6f90cbbc811fc9d35256d4 Mon Sep 17 00:00:00 2001 From: LM Date: Fri, 10 Jun 2011 23:18:05 +0200 Subject: [PATCH 004/131] Minor compile fixes for v1.0 --- qgroundcontrol.pri | 4 ++-- src/comm/MAVLinkSimulationLink.cc | 8 ++++---- src/ui/map3D/Pixhawk3DWidget.cc | 18 +++++++++--------- src/ui/map3D/WaypointGroupNode.cc | 4 ++-- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index f79a29984d..407bc98589 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -221,7 +221,7 @@ message("Compiling for linux 32") DEFINES += QGC_LIBFREENECT_ENABLED } - QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$DESTDIR + QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$DESTDIR QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$DESTDIR QMAKE_POST_LINK += && mkdir -p $$DESTDIR/images QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$DESTDIR/images/Vera.ttf @@ -295,7 +295,7 @@ linux-g++-64 { DEFINES += QGC_LIBFREENECT_ENABLED } - QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$DESTDIR + QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$DESTDIR QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$DESTDIR QMAKE_POST_LINK += && mkdir -p $$DESTDIR/images QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$DESTDIR/images/Vera.ttf diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 3f716b0d8f..c0cc9a3a1b 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -757,7 +757,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { if (j != 5) { // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), 0, onboardParams.size(), j); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), MAVLINK_TYPE_FLOAT, onboardParams.size(), j); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -785,7 +785,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) onboardParams.insert(key, set.param_value); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, 0, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, MAVLINK_TYPE_FLOAT, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -806,7 +806,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, 0, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAVLINK_TYPE_FLOAT, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -818,7 +818,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, 0, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAVLINK_TYPE_FLOAT, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index d441d87a99..009e962cb5 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -139,7 +139,7 @@ Pixhawk3DWidget::selectFrame(QString text) if (text.compare("Global") == 0) { frame = MAV_FRAME_GLOBAL; } else if (text.compare("Local") == 0) { - frame = MAV_FRAME_LOCAL; + frame = MAV_FRAME_LOCAL_NED; } getPosition(lastRobotX, lastRobotY, lastRobotZ); @@ -231,7 +231,7 @@ Pixhawk3DWidget::selectTarget(void) getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); target.set(cursorWorldCoords.first, cursorWorldCoords.second); - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { double z = uas->getLocalZ(); std::pair cursorWorldCoords = @@ -266,7 +266,7 @@ Pixhawk3DWidget::insertWaypoint(void) latitude, longitude); wp = new Waypoint(0, longitude, latitude, altitude); - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { double z = uas->getLocalZ(); std::pair cursorWorldCoords = @@ -314,7 +314,7 @@ Pixhawk3DWidget::setWaypoint(void) waypoint->setX(longitude); waypoint->setY(latitude); waypoint->setZ(altitude); - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { double z = uas->getLocalZ(); std::pair cursorWorldCoords = @@ -345,7 +345,7 @@ Pixhawk3DWidget::setWaypointAltitude(void) Waypoint* waypoint = waypoints.at(selectedWpIndex); double altitude = waypoint->getZ(); - if (frame == MAV_FRAME_LOCAL) { + if (frame == MAV_FRAME_LOCAL_NED) { altitude = -altitude; } @@ -355,7 +355,7 @@ Pixhawk3DWidget::setWaypointAltitude(void) if (ok) { if (frame == MAV_FRAME_GLOBAL) { waypoint->setZ(newAltitude); - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { waypoint->setZ(-newAltitude); } } @@ -640,7 +640,7 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z, Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); z = -altitude; - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { x = uas->getLocalX(); y = uas->getLocalY(); z = uas->getLocalZ(); @@ -673,7 +673,7 @@ Pixhawk3DWidget::getPosition(double& x, double& y, double& z, Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); z = -altitude; - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { x = uas->getLocalX(); y = uas->getLocalY(); z = uas->getLocalZ(); @@ -941,7 +941,7 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, oss.precision(6); oss << " Cursor [" << cursorLatitude << " " << cursorLongitude << "]"; - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { oss << " x = " << robotX << " y = " << robotY << " z = " << robotZ << diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index 5dc99cc585..ffacfc42f0 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -63,7 +63,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) QString utmZone; Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone); robotZ = -altitude; - } else if (frame == MAV_FRAME_LOCAL) { + } else if (frame == MAV_FRAME_LOCAL_NED) { robotX = uas->getLocalX(); robotY = uas->getLocalY(); robotZ = uas->getLocalZ(); @@ -154,7 +154,7 @@ WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z) QString utmZone; Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); z = -altitude; - } else if (wp->getFrame() == MAV_FRAME_LOCAL) { + } else if (wp->getFrame() == MAV_FRAME_LOCAL_NED) { x = wp->getX(); y = wp->getY(); z = wp->getZ(); -- GitLab From eda637f58fc3c4053b9c6e10030d9c48d4a01fe6 Mon Sep 17 00:00:00 2001 From: fattony Date: Fri, 29 Jul 2011 11:52:24 +0200 Subject: [PATCH 005/131] enabled all four video streams with image id check disabled hud instruments at startup --- src/apps/qgcvideo/QGCVideoMainWindow.cc | 187 ++++++++++++++++++------ src/apps/qgcvideo/QGCVideoWidget.cc | 2 +- 2 files changed, 141 insertions(+), 48 deletions(-) diff --git a/src/apps/qgcvideo/QGCVideoMainWindow.cc b/src/apps/qgcvideo/QGCVideoMainWindow.cc index 6aeab61fc0..0fe36fb9b7 100644 --- a/src/apps/qgcvideo/QGCVideoMainWindow.cc +++ b/src/apps/qgcvideo/QGCVideoMainWindow.cc @@ -37,7 +37,10 @@ QByteArray imageRecBuffer1 = QByteArray(376*240,255); QByteArray imageRecBuffer2 = QByteArray(376*240,255); + QByteArray imageRecBuffer3 = QByteArray(376*240,255); + QByteArray imageRecBuffer4 = QByteArray(376*240,255); static int part = 0; + unsigned char last_id = 0; QGCVideoMainWindow::QGCVideoMainWindow(QWidget *parent) : QMainWindow(parent), @@ -75,6 +78,7 @@ void QGCVideoMainWindow::receiveBytes(LinkInterface* link, QByteArray data) QString bytes; QString index; + QString imageid; QString ascii; @@ -93,90 +97,147 @@ void QGCVideoMainWindow::receiveBytes(LinkInterface* link, QByteArray data) header = header.arg(imgWidth).arg(imgHeight).arg(imgColors); unsigned char i0 = data[0]; + unsigned char id = data[1]; + + index.append(QString().sprintf("%02x", i0)); + imageid.append(QString().sprintf("%02x", id)); + qDebug() << "Received" << data.size() << "bytes"<< " part: " <video1Widget->copyImage(test); - ui->video2Widget->copyImage(image1); - ui->video3Widget->copyImage(image2); - //ui->video4Widget->copyImage(test); + ui->video1Widget->copyImage(image1); + ui->video2Widget->copyImage(image2); + ui->video3Widget->copyImage(image3); + ui->video4Widget->copyImage(image4); part = 0; imageRecBuffer1.clear(); imageRecBuffer2.clear(); + imageRecBuffer3.clear(); + imageRecBuffer4.clear(); } - index.append(QString().sprintf("%02x ", i0)); - for (int j=0; j 31 && data.at(j) < 127) @@ -240,9 +334,8 @@ void QGCVideoMainWindow::receiveBytes(LinkInterface* link, QByteArray data) ascii.append(219); } - } - qDebug() << "Received" << data.size() << "bytes"; - qDebug() << "index: " < Date: Sun, 31 Jul 2011 17:22:29 +0200 Subject: [PATCH 006/131] Updated to MAVLink v1.0 --- .../include/ardupilotmega/ardupilotmega.h | 9 +- .../mavlink/include/ardupilotmega/mavlink.h | 8 +- thirdParty/mavlink/include/checksum.h | 44 + thirdParty/mavlink/include/common/common.h | 203 +- thirdParty/mavlink/include/common/mavlink.h | 8 +- .../include/common/mavlink_msg_action.h | 92 +- .../include/common/mavlink_msg_action_ack.h | 82 +- .../include/common/mavlink_msg_attitude.h | 171 +- .../mavlink_msg_attitude_controller_output.h | 112 +- .../include/common/mavlink_msg_auth_key.h | 76 +- .../mavlink/include/common/mavlink_msg_boot.h | 77 +- .../mavlink_msg_change_operator_control.h | 106 +- .../mavlink_msg_change_operator_control_ack.h | 92 +- .../include/common/mavlink_msg_command.h | 162 +- .../include/common/mavlink_msg_command_ack.h | 92 +- .../common/mavlink_msg_control_status.h | 142 +- .../include/common/mavlink_msg_debug.h | 87 +- .../include/common/mavlink_msg_debug_vect.h | 140 +- .../common/mavlink_msg_global_position.h | 171 +- .../common/mavlink_msg_global_position_int.h | 146 +- .../common/mavlink_msg_gps_local_origin_set.h | 107 +- .../include/common/mavlink_msg_gps_raw.h | 196 +- .../include/common/mavlink_msg_gps_raw_int.h | 196 +- .../mavlink_msg_gps_set_global_origin.h | 127 +- .../include/common/mavlink_msg_gps_status.h | 158 +- .../include/common/mavlink_msg_heartbeat.h | 92 +- .../common/mavlink_msg_local_position.h | 171 +- .../mavlink_msg_local_position_setpoint.h | 122 +- .../mavlink_msg_local_position_setpoint_set.h | 142 +- .../common/mavlink_msg_manual_control.h | 172 +- .../common/mavlink_msg_named_value_float.h | 91 +- .../common/mavlink_msg_named_value_int.h | 91 +- .../mavlink_msg_nav_controller_output.h | 176 +- .../common/mavlink_msg_param_request_list.h | 82 +- .../common/mavlink_msg_param_request_read.h | 117 +- .../include/common/mavlink_msg_param_set.h | 119 +- .../include/common/mavlink_msg_param_value.h | 125 +- .../mavlink/include/common/mavlink_msg_ping.h | 116 +- .../mavlink_msg_position_controller_output.h | 112 +- .../common/mavlink_msg_position_target.h | 122 +- .../include/common/mavlink_msg_raw_imu.h | 202 +- .../include/common/mavlink_msg_raw_pressure.h | 133 +- .../common/mavlink_msg_rc_channels_raw.h | 176 +- .../common/mavlink_msg_rc_channels_scaled.h | 176 +- .../common/mavlink_msg_request_data_stream.h | 115 +- .../common/mavlink_msg_safety_allowed_area.h | 162 +- .../mavlink_msg_safety_set_allowed_area.h | 182 +- .../include/common/mavlink_msg_scaled_imu.h | 202 +- .../common/mavlink_msg_scaled_pressure.h | 124 +- .../common/mavlink_msg_servo_output_raw.h | 166 +- .../include/common/mavlink_msg_set_altitude.h | 87 +- .../include/common/mavlink_msg_set_mode.h | 82 +- .../include/common/mavlink_msg_set_nav_mode.h | 82 +- .../common/mavlink_msg_state_correction.h | 197 +- .../include/common/mavlink_msg_statustext.h | 94 +- .../include/common/mavlink_msg_sys_status.h | 144 +- .../include/common/mavlink_msg_system_time.h | 81 +- .../common/mavlink_msg_system_time_utc.h | 92 +- .../include/common/mavlink_msg_vfr_hud.h | 148 +- .../include/common/mavlink_msg_waypoint.h | 244 +- .../include/common/mavlink_msg_waypoint_ack.h | 92 +- .../common/mavlink_msg_waypoint_clear_all.h | 82 +- .../common/mavlink_msg_waypoint_count.h | 95 +- .../common/mavlink_msg_waypoint_current.h | 75 +- .../common/mavlink_msg_waypoint_reached.h | 75 +- .../common/mavlink_msg_waypoint_request.h | 95 +- .../mavlink_msg_waypoint_request_list.h | 82 +- .../common/mavlink_msg_waypoint_set_current.h | 95 +- thirdParty/mavlink/include/mavlink_types.h | 198 +- thirdParty/mavlink/include/minimal/mavlink.h | 8 +- .../include/minimal/mavlink_msg_heartbeat.h | 92 +- thirdParty/mavlink/include/minimal/minimal.h | 9 +- thirdParty/mavlink/include/pixhawk/mavlink.h | 8 +- .../pixhawk/mavlink_msg_attitude_control.h | 172 +- .../include/pixhawk/mavlink_msg_aux_status.h | 140 +- .../pixhawk/mavlink_msg_brief_feature.h | 180 +- .../mavlink_msg_data_transmission_handshake.h | 117 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 89 +- .../pixhawk/mavlink_msg_image_available.h | 407 ++-- .../mavlink_msg_image_trigger_control.h | 72 +- .../pixhawk/mavlink_msg_image_triggered.h | 250 ++- .../include/pixhawk/mavlink_msg_marker.h | 165 +- .../pixhawk/mavlink_msg_pattern_detected.h | 119 +- .../pixhawk/mavlink_msg_point_of_interest.h | 172 +- ...mavlink_msg_point_of_interest_connection.h | 221 +- .../mavlink_msg_position_control_offset_set.h | 142 +- .../mavlink_msg_position_control_setpoint.h | 135 +- ...avlink_msg_position_control_setpoint_set.h | 155 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 155 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 133 +- .../mavlink_msg_vicon_position_estimate.h | 171 +- .../mavlink_msg_vision_position_estimate.h | 171 +- .../mavlink_msg_vision_speed_estimate.h | 126 +- .../pixhawk/mavlink_msg_watchdog_command.h | 108 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 88 +- .../mavlink_msg_watchdog_process_info.h | 141 +- .../mavlink_msg_watchdog_process_status.h | 136 +- thirdParty/mavlink/include/pixhawk/pixhawk.h | 11 +- thirdParty/mavlink/include/protocol.h | 299 ++- thirdParty/mavlink/include/slugs/mavlink.h | 8 +- .../include/slugs/mavlink_msg_air_data.h | 105 +- .../include/slugs/mavlink_msg_cpu_load.h | 95 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 85 +- .../include/slugs/mavlink_msg_data_log.h | 152 +- .../include/slugs/mavlink_msg_diagnostic.h | 146 +- .../include/slugs/mavlink_msg_gps_date_time.h | 132 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 117 +- .../include/slugs/mavlink_msg_sensor_bias.h | 152 +- .../include/slugs/mavlink_msg_slugs_action.h | 95 +- .../slugs/mavlink_msg_slugs_navigation.h | 187 +- thirdParty/mavlink/include/slugs/slugs.h | 9 +- thirdParty/mavlink/include/ualberta/mavlink.h | 8 +- .../ualberta/mavlink_msg_nav_filter_bias.h | 171 +- .../ualberta/mavlink_msg_radio_calibration.h | 146 +- .../mavlink_msg_request_rc_channels.h | 101 - .../mavlink_msg_ualberta_sys_status.h | 92 +- .../mavlink/include/ualberta/ualberta.h | 15 +- .../mavlink/message_definitions/common.xml | 1997 +++++++++-------- .../mavlink/message_definitions/pixhawk.xml | 12 +- .../mavlink/missionlib/testing/.gitignore | 2 + thirdParty/mavlink/missionlib/testing/udp.c | 213 ++ 121 files changed, 10424 insertions(+), 6240 deletions(-) delete mode 100644 thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h create mode 100644 thirdParty/mavlink/missionlib/testing/.gitignore create mode 100644 thirdParty/mavlink/missionlib/testing/udp.c diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 4d13e0b543..d03060c3d9 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -33,13 +33,6 @@ extern "C" { // MESSAGE DEFINITIONS - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 8ee2430d74..ba99d28cc3 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "ardupilotmega.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/checksum.h b/thirdParty/mavlink/include/checksum.h index c16a06e1b9..07bab9102a 100644 --- a/thirdParty/mavlink/include/checksum.h +++ b/thirdParty/mavlink/include/checksum.h @@ -34,6 +34,7 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) tmp=data ^ (uint8_t)(*crcAccum &0xff); tmp^= (tmp<<4); *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test } /** @@ -86,6 +87,49 @@ static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) } +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + #endif /* _CHECKSUM_H_ */ diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index bf99ef0003..39740c7af8 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef COMMON_H #define COMMON_H @@ -18,17 +18,180 @@ extern "C" { // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 +#define MAVLINK_VERSION 3 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 +#define MAVLINK_VERSION 3 #endif // ENUM DEFINITIONS -/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +enum MAV_CLASS +{ + MAV_CLASS_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_CLASS_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */ + MAV_CLASS_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_CLASS_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */ + MAV_CLASS_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_CLASS_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_CLASS_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_CLASS_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_CLASS_UDB=10, /* UAV Dev Board | */ + MAV_CLASS_FP=11, /* FlexiPilot | */ + MAV_CLASS_ENUM_END +}; + +/** @brief */ +enum MAV_MODE +{ + MAV_MODE_UNINIT=0, /* System is in undefined state | */ + MAV_MODE_LOCKED=1, /* Motors are blocked, system is safe | */ + MAV_MODE_MANUAL=2, /* System is allowed to be active, under manual (RC) control | */ + MAV_MODE_GUIDED=3, /* System is allowed to be active, under autonomous control, manual setpoint | */ + MAV_MODE_AUTO=4, /* System is allowed to be active, under autonomous control and navigation | */ + MAV_MODE_TEST1=5, /* Generic test mode, for custom use | */ + MAV_MODE_TEST2=6, /* Generic test mode, for custom use | */ + MAV_MODE_TEST3=7, /* Generic test mode, for custom use | */ + MAV_MODE_READY=8, /* System is ready, motors are unblocked, but controllers are inactive | */ + MAV_MODE_RC_TRAINING=9, /* System is blocked, only RC valued are read and reported back | */ + MAV_MODE_ENUM_END +}; + +/** @brief */ +enum MAV_NAV +{ + MAV_NAV_GROUNDED=0, /* System is currently on ground. | */ + MAV_NAV_LIFTOFF=1, /* System is during liftoff, not in normal navigation mode yet. | */ + MAV_NAV_HOLD=2, /* System is holding its current position (rotorcraft or rover / boat). | */ + MAV_NAV_WAYPOINT=3, /* System is navigating towards the next waypoint. | */ + MAV_NAV_VECTOR=4, /* System is flying at a defined course and speed. | */ + MAV_NAV_RETURNING=5, /* System is return straight to home position. | */ + MAV_NAV_LANDING=6, /* System is landing. | */ + MAV_NAV_LOST=7, /* System lost its position input and is in attitude / flight stabilization only. | */ + MAV_NAV_LOITER=8, /* System is loitering in wait position. DO NOT USE THIS FOR WAYPOINT LOITER! | */ + MAV_NAV_ENUM_END +}; + +/** @brief */ +enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_ENUM_END +}; + +/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */ +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 +}; + +/** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */ +enum MAV_ROI +{ + MAV_ROI_WPNEXT=0, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=1, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=2, /* Point toward fixed location. | */ + MAV_ROI_TARGET=3, /* Point toward of given id. | */ + MAV_ROI_ENUM_END +}; + +/** @brief */ +enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_GROUND=5, /* Ground installation | */ + MAV_TYPE_OCU=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_UGV_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_UGV_SURFACE_SHIP=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_UGV_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_ENUM_END +}; + +/** @brief */ +enum MAV_COMPONENT +{ + MAV_COMP_ID_GPS=220, + MAV_COMP_ID_WAYPOINTPLANNER=190, + MAV_COMP_ID_PATHPLANNER=195, + MAV_COMP_ID_MAPPER=180, + MAV_COMP_ID_CAMERA=100, + MAV_COMP_ID_IMU=200, + MAV_COMP_ID_IMU_2=201, + MAV_COMP_ID_IMU_3=202, + MAV_COMP_ID_UDP_BRIDGE=240, + MAV_COMP_ID_UART_BRIDGE=241, + MAV_COMP_ID_SYSTEM_CONTROL=250, + MAV_COMP_ID_SERVO1=140, + MAV_COMP_ID_SERVO2=141, + MAV_COMP_ID_SERVO3=142, + MAV_COMP_ID_SERVO4=143, + MAV_COMP_ID_SERVO5=144, + MAV_COMP_ID_SERVO6=145, + MAV_COMP_ID_SERVO7=146, + MAV_COMP_ID_SERVO8=147, + MAV_COMP_ID_SERVO9=148, + MAV_COMP_ID_SERVO10=149, + MAV_COMP_ID_SERVO11=150, + MAV_COMP_ID_SERVO12=151, + MAV_COMP_ID_SERVO13=152, + MAV_COMP_ID_SERVO14=153, + MAV_COMPONENT_ENUM_END +}; + +/** @brief */ +enum MAV_FRAME +{ + MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. | */ + MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */ + MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground. | */ + MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ + MAV_FRAME_ENUM_END +}; + +/** @brief */ +enum MAVLINK_DATA_STREAM_TYPE +{ + MAVLINK_DATA_STREAM_IMG_JPEG=0, + MAVLINK_DATA_STREAM_IMG_BMP=1, + MAVLINK_DATA_STREAM_IMG_RAW8U=2, + MAVLINK_DATA_STREAM_IMG_RAW32U=3, + MAVLINK_DATA_STREAM_IMG_PGM=4, + MAVLINK_DATA_STREAM_IMG_PNG=5, + MAVLINK_DATA_STREAM_TYPE_ENUM_END +}; + +/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ enum MAV_CMD { MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. | Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) | Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) | 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. | Desired yaw angle at waypoint (rotary wing) | Latitude | Longitude | Altitude | */ @@ -62,31 +225,6 @@ enum MAV_CMD MAV_CMD_ENUM_END }; -/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */ -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 -}; - -/** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */ -enum MAV_ROI -{ - MAV_ROI_WPNEXT=0, /* Point toward next waypoint. | */ - MAV_ROI_WPINDEX=1, /* Point toward given waypoint. | */ - MAV_ROI_LOCATION=2, /* Point toward fixed location. | */ - MAV_ROI_TARGET=3, /* Point toward of given id. | */ - MAV_ROI_ENUM_END -}; - // MESSAGE DEFINITIONS @@ -153,13 +291,6 @@ enum MAV_ROI #include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 404b970bff..ef3973fbbc 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "common.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action.h b/thirdParty/mavlink/include/common/mavlink_msg_action.h index f1de546754..82ec1f50eb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action.h @@ -1,6 +1,8 @@ // MESSAGE ACTION PACKING #define MAVLINK_MSG_ID_ACTION 10 +#define MAVLINK_MSG_ID_ACTION_LEN 3 +#define MAVLINK_MSG_10_LEN 3 typedef struct __mavlink_action_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_action_t } mavlink_action_t; - - /** * @brief Pack a action message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_action_t */ static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action) { - uint16_t i = 0; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action - i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action - i += put_uint8_t_by_index(action, i, msg->payload); // The action id + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTION_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t compon */ static inline uint16_t mavlink_msg_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action) { - uint16_t i = 0; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action - i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action - i += put_uint8_t_by_index(action, i, msg->payload); // The action id + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTION_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t comp * @param action The action id */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) { mavlink_message_t msg; - mavlink_msg_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, target_component, action); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_action_t *p = (mavlink_action_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ACTION_LEN; + msg.msgid = MAVLINK_MSG_ID_ACTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) +{ + mavlink_header_t hdr; + mavlink_action_t payload; + uint16_t checksum; + mavlink_action_t *p = &payload; + + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ACTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_ACTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t targe */ static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,7 +169,8 @@ static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_mess */ static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; + return (uint8_t)(p->action); } /** @@ -129,7 +181,5 @@ static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg */ static inline void mavlink_msg_action_decode(const mavlink_message_t* msg, mavlink_action_t* action) { - action->target = mavlink_msg_action_get_target(msg); - action->target_component = mavlink_msg_action_get_target_component(msg); - action->action = mavlink_msg_action_get_action(msg); + memcpy( action, msg->payload, sizeof(mavlink_action_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h index 6e93449e20..bfb41a8194 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h @@ -1,6 +1,8 @@ // MESSAGE ACTION_ACK PACKING #define MAVLINK_MSG_ID_ACTION_ACK 9 +#define MAVLINK_MSG_ID_ACTION_ACK_LEN 2 +#define MAVLINK_MSG_9_LEN 2 typedef struct __mavlink_action_ack_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_action_ack_t } mavlink_action_ack_t; - - /** * @brief Pack a action_ack message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_action_ack_t */ static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t action, uint8_t result) { - uint16_t i = 0; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - i += put_uint8_t_by_index(result, i, msg->payload); // 0: Action DENIED, 1: Action executed + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTION_ACK_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_action_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t action, uint8_t result) { - uint16_t i = 0; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - i += put_uint8_t_by_index(result, i, msg->payload); // 0: Action DENIED, 1: Action executed + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTION_ACK_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t * @param result 0: Action DENIED, 1: Action executed */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) { mavlink_message_t msg; - mavlink_msg_action_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, action, result); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg.payload[0]; + + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_ACTION_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) +{ + mavlink_header_t hdr; + mavlink_action_ack_t payload; + uint16_t checksum; + mavlink_action_ack_t *p = &payload; + + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_ACTION_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t a */ static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; + return (uint8_t)(p->action); } /** @@ -102,7 +150,8 @@ static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* */ static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; + return (uint8_t)(p->result); } /** @@ -113,6 +162,5 @@ static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* */ static inline void mavlink_msg_action_ack_decode(const mavlink_message_t* msg, mavlink_action_ack_t* action_ack) { - action_ack->action = mavlink_msg_action_ack_get_action(msg); - action_ack->result = mavlink_msg_action_ack_get_result(msg); + memcpy( action_ack, msg->payload, sizeof(mavlink_action_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 6daba3c54b..5d395fda6c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE PACKING #define MAVLINK_MSG_ID_ATTITUDE 30 +#define MAVLINK_MSG_ID_ATTITUDE_LEN 32 +#define MAVLINK_MSG_30_LEN 32 typedef struct __mavlink_attitude_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_attitude_t } mavlink_attitude_t; - - /** * @brief Pack a attitude message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_attitude_t */ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { - uint16_t i = 0; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { - uint16_t i = 0; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co * @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_message_t msg; - mavlink_msg_attitude_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ATTITUDE_LEN; + msg.msgid = MAVLINK_MSG_ID_ATTITUDE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_attitude_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t us */ static inline uint64_t mavlink_msg_attitude_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_attitude_get_usec(const mavlink_message_t* ms */ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->rollspeed); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->pitchspeed); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->yawspeed); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* m */ static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude) { - attitude->usec = mavlink_msg_attitude_get_usec(msg); - attitude->roll = mavlink_msg_attitude_get_roll(msg); - attitude->pitch = mavlink_msg_attitude_get_pitch(msg); - attitude->yaw = mavlink_msg_attitude_get_yaw(msg); - attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg); - attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); - attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); + memcpy( attitude, msg->payload, sizeof(mavlink_attitude_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h index 60fa775540..081dd82d64 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT 60 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN 5 +#define MAVLINK_MSG_60_LEN 5 typedef struct __mavlink_attitude_controller_output_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_attitude_controller_output_t } mavlink_attitude_controller_output_t; - - /** * @brief Pack a attitude_controller_output message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_attitude_controller_output_t */ static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { - uint16_t i = 0; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t syste */ static inline uint16_t mavlink_msg_attitude_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { - uint16_t i = 0; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t sys * @param thrust Attitude thrust: -128: -100%, 127: +100% */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { mavlink_message_t msg; - mavlink_msg_attitude_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, roll, pitch, yaw, thrust); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg.payload[0]; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; + msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) +{ + mavlink_header_t hdr; + mavlink_attitude_controller_output_t payload; + uint16_t checksum; + mavlink_attitude_controller_output_t *p = &payload; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (uint8_t)(p->enabled); } /** @@ -120,7 +174,8 @@ static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const m */ static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->roll); } /** @@ -130,7 +185,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavli */ static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->pitch); } /** @@ -140,7 +196,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavl */ static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->yaw); } /** @@ -150,7 +207,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlin */ static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->thrust); } /** @@ -161,9 +219,5 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mav */ static inline void mavlink_msg_attitude_controller_output_decode(const mavlink_message_t* msg, mavlink_attitude_controller_output_t* attitude_controller_output) { - attitude_controller_output->enabled = mavlink_msg_attitude_controller_output_get_enabled(msg); - attitude_controller_output->roll = mavlink_msg_attitude_controller_output_get_roll(msg); - attitude_controller_output->pitch = mavlink_msg_attitude_controller_output_get_pitch(msg); - attitude_controller_output->yaw = mavlink_msg_attitude_controller_output_get_yaw(msg); - attitude_controller_output->thrust = mavlink_msg_attitude_controller_output_get_thrust(msg); + memcpy( attitude_controller_output, msg->payload, sizeof(mavlink_attitude_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index 605ba7db61..f857ed91a7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -1,16 +1,16 @@ // MESSAGE AUTH_KEY PACKING #define MAVLINK_MSG_ID_AUTH_KEY 7 +#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 +#define MAVLINK_MSG_7_LEN 32 typedef struct __mavlink_auth_key_t { char key[32]; ///< key } mavlink_auth_key_t; - #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 - /** * @brief Pack a auth_key message * @param system_id ID of this system @@ -22,12 +22,12 @@ typedef struct __mavlink_auth_key_t */ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* key) { - uint16_t i = 0; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); } /** @@ -41,12 +41,12 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* key) { - uint16_t i = 0; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); } /** @@ -69,12 +69,57 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co * @param key key */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) { mavlink_message_t msg; - mavlink_msg_auth_key_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, key); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg.payload[0]; + + memcpy(p->key, key, sizeof(p->key)); // char[32]:key + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; + msg.msgid = MAVLINK_MSG_ID_AUTH_KEY; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) +{ + mavlink_header_t hdr; + mavlink_auth_key_t payload; + uint16_t checksum; + mavlink_auth_key_t *p = &payload; + + memcpy(p->key, key, sizeof(p->key)); // char[32]:key + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; + hdr.msgid = MAVLINK_MSG_ID_AUTH_KEY; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -85,11 +130,12 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* * * @return key */ -static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char* key) { + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*32); - return sizeof(char)*32; + memcpy(key, p->key, sizeof(p->key)); + return sizeof(p->key); } /** @@ -100,5 +146,5 @@ static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg */ static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) { - mavlink_msg_auth_key_get_key(msg, auth_key->key); + memcpy( auth_key, msg->payload, sizeof(mavlink_auth_key_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 69d8ed64a7..00e13e2913 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -1,6 +1,8 @@ // MESSAGE BOOT PACKING #define MAVLINK_MSG_ID_BOOT 1 +#define MAVLINK_MSG_ID_BOOT_LEN 4 +#define MAVLINK_MSG_1_LEN 4 typedef struct __mavlink_boot_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_boot_t } mavlink_boot_t; - - /** * @brief Pack a boot message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_boot_t */ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t version) { - uint16_t i = 0; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version + p->version = version; // uint32_t:The onboard software version - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BOOT_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen */ static inline uint16_t mavlink_msg_boot_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t version) { - uint16_t i = 0; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version + p->version = version; // uint32_t:The onboard software version - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BOOT_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon * @param version The onboard software version */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { mavlink_message_t msg; - mavlink_msg_boot_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, version); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_boot_t *p = (mavlink_boot_t *)&msg.payload[0]; + + p->version = version; // uint32_t:The onboard software version + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_BOOT_LEN; + msg.msgid = MAVLINK_MSG_ID_BOOT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) +{ + mavlink_header_t hdr; + mavlink_boot_t payload; + uint16_t checksum; + mavlink_boot_t *p = &payload; + + p->version = version; // uint32_t:The onboard software version + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_BOOT_LEN; + hdr.msgid = MAVLINK_MSG_ID_BOOT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,12 +131,8 @@ static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t versio */ static inline uint32_t mavlink_msg_boot_get_version(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; + return (uint32_t)(p->version); } /** @@ -102,5 +143,5 @@ static inline uint32_t mavlink_msg_boot_get_version(const mavlink_message_t* msg */ static inline void mavlink_msg_boot_decode(const mavlink_message_t* msg, mavlink_boot_t* boot) { - boot->version = mavlink_msg_boot_get_version(msg); + memcpy( boot, msg->payload, sizeof(mavlink_boot_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h index 3eab478cbf..fdc783caca 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -1,6 +1,8 @@ // MESSAGE CHANGE_OPERATOR_CONTROL PACKING #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 +#define MAVLINK_MSG_5_LEN 28 typedef struct __mavlink_change_operator_control_t { @@ -10,10 +12,8 @@ typedef struct __mavlink_change_operator_control_t char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" } mavlink_change_operator_control_t; - #define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 - /** * @brief Pack a change_operator_control message * @param system_id ID of this system @@ -28,15 +28,15 @@ typedef struct __mavlink_change_operator_control_t */ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { - uint16_t i = 0; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System the GCS requests control for - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(version, i, msg->payload); // 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - i += put_array_by_index((const int8_t*)passkey, sizeof(char)*25, i, msg->payload); // Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } /** @@ -53,15 +53,15 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { - uint16_t i = 0; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System the GCS requests control for - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(version, i, msg->payload); // 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - i += put_array_by_index((const int8_t*)passkey, sizeof(char)*25, i, msg->payload); // Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { mavlink_message_t msg; - mavlink_msg_change_operator_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, control_request, version, passkey); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg.payload[0]; + + 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 "!?,.-" + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_change_operator_control_t *p = &payload; + + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +167,8 @@ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(cons */ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->control_request); } /** @@ -125,7 +178,8 @@ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(co */ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->version); } /** @@ -133,11 +187,12 @@ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavl * * @return Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ -static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char* passkey) { + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t), sizeof(char)*25); - return sizeof(char)*25; + memcpy(passkey, p->passkey, sizeof(p->passkey)); + return sizeof(p->passkey); } /** @@ -148,8 +203,5 @@ static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mav */ static inline void mavlink_msg_change_operator_control_decode(const mavlink_message_t* msg, mavlink_change_operator_control_t* change_operator_control) { - change_operator_control->target_system = mavlink_msg_change_operator_control_get_target_system(msg); - change_operator_control->control_request = mavlink_msg_change_operator_control_get_control_request(msg); - change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg); - mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey); + memcpy( change_operator_control, msg->payload, sizeof(mavlink_change_operator_control_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h index 3529ae0591..b49883dd2d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h @@ -1,6 +1,8 @@ // MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 +#define MAVLINK_MSG_6_LEN 3 typedef struct __mavlink_change_operator_control_ack_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_change_operator_control_ack_t } mavlink_change_operator_control_ack_t; - - /** * @brief Pack a change_operator_control_ack message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_change_operator_control_ack_t */ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { - uint16_t i = 0; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - i += put_uint8_t_by_index(gcs_system_id, i, msg->payload); // ID of the GCS this message - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(ack, i, msg->payload); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst */ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { - uint16_t i = 0; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - i += put_uint8_t_by_index(gcs_system_id, i, msg->payload); // ID of the GCS this message - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(ack, i, msg->payload); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy * @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_message_t msg; - mavlink_msg_change_operator_control_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, gcs_system_id, control_request, ack); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_change_operator_control_ack_t *p = &payload; + + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->gcs_system_id); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id( */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->control_request); } /** @@ -118,7 +169,8 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_reques */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->ack); } /** @@ -129,7 +181,5 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavl */ static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_message_t* msg, mavlink_change_operator_control_ack_t* change_operator_control_ack) { - change_operator_control_ack->gcs_system_id = mavlink_msg_change_operator_control_ack_get_gcs_system_id(msg); - change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg); - change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg); + memcpy( change_operator_control_ack, msg->payload, sizeof(mavlink_change_operator_control_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index dbc1235707..c22eb79833 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -1,6 +1,8 @@ // MESSAGE COMMAND PACKING #define MAVLINK_MSG_ID_COMMAND 75 +#define MAVLINK_MSG_ID_COMMAND_LEN 20 +#define MAVLINK_MSG_75_LEN 20 typedef struct __mavlink_command_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_command_t } mavlink_command_t; - - /** * @brief Pack a command message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_command_t */ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { - uint16_t i = 0; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components - i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum. - i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum. - i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum. - i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum. - i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { - uint16_t i = 0; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components - i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum. - i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum. - i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum. - i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum. - i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param4 Parameter 4, as defined by MAV_CMD enum. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { mavlink_message_t msg; - mavlink_msg_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, command, confirmation, param1, param2, param3, param4); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_command_t *p = (mavlink_command_t *)&msg.payload[0]; + + 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. + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_COMMAND_LEN; + msg.msgid = MAVLINK_MSG_ID_COMMAND; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) +{ + mavlink_header_t hdr; + mavlink_command_t payload; + uint16_t checksum; + mavlink_command_t *p = &payload; + + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_COMMAND_LEN; + hdr.msgid = MAVLINK_MSG_ID_COMMAND; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,7 +187,8 @@ static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t targ */ static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -138,7 +198,8 @@ static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_messag */ static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -148,7 +209,8 @@ static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_mes */ static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->command); } /** @@ -158,7 +220,8 @@ static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->confirmation); } /** @@ -168,12 +231,8 @@ static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message */ static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param1); } /** @@ -183,12 +242,8 @@ static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param2); } /** @@ -198,12 +253,8 @@ static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param3); } /** @@ -213,12 +264,8 @@ static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param4); } /** @@ -229,12 +276,5 @@ static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) */ static inline void mavlink_msg_command_decode(const mavlink_message_t* msg, mavlink_command_t* command) { - command->target_system = mavlink_msg_command_get_target_system(msg); - command->target_component = mavlink_msg_command_get_target_component(msg); - command->command = mavlink_msg_command_get_command(msg); - command->confirmation = mavlink_msg_command_get_confirmation(msg); - command->param1 = mavlink_msg_command_get_param1(msg); - command->param2 = mavlink_msg_command_get_param2(msg); - command->param3 = mavlink_msg_command_get_param3(msg); - command->param4 = mavlink_msg_command_get_param4(msg); + memcpy( command, msg->payload, sizeof(mavlink_command_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index 32b39da3a6..a82703d535 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -1,6 +1,8 @@ // MESSAGE COMMAND_ACK PACKING #define MAVLINK_MSG_ID_COMMAND_ACK 76 +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 +#define MAVLINK_MSG_76_LEN 8 typedef struct __mavlink_command_ack_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_command_ack_t } mavlink_command_ack_t; - - /** * @brief Pack a command_ack message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_command_ack_t */ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float command, float result) { - uint16_t i = 0; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float command, float result) { - uint16_t i = 0; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t * @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_message_t msg; - mavlink_msg_command_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, command, result); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_COMMAND_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_command_ack_t *p = &payload; + + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_COMMAND_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,12 +139,8 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float co */ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; + return (float)(p->command); } /** @@ -107,12 +150,8 @@ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* */ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; + return (float)(p->result); } /** @@ -123,6 +162,5 @@ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* */ static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack) { - command_ack->command = mavlink_msg_command_ack_get_command(msg); - command_ack->result = mavlink_msg_command_ack_get_result(msg); + memcpy( command_ack, msg->payload, sizeof(mavlink_command_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 746e889aaf..00749b618e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -1,6 +1,8 @@ // MESSAGE CONTROL_STATUS PACKING #define MAVLINK_MSG_ID_CONTROL_STATUS 52 +#define MAVLINK_MSG_ID_CONTROL_STATUS_LEN 8 +#define MAVLINK_MSG_52_LEN 8 typedef struct __mavlink_control_status_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_control_status_t } mavlink_control_status_t; - - /** * @brief Pack a control_status message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_control_status_t */ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - uint16_t i = 0; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - uint16_t i = 0; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint * @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_message_t msg; - mavlink_msg_control_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_control_status_t *p = &payload; + + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,7 +187,8 @@ static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->position_fix); } /** @@ -138,7 +198,8 @@ static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_ */ static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->vision_fix); } /** @@ -148,7 +209,8 @@ static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_me */ static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->gps_fix); } /** @@ -158,7 +220,8 @@ static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_messa */ static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->ahrs_health); } /** @@ -168,7 +231,8 @@ static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_m */ static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_att); } /** @@ -178,7 +242,8 @@ static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_m */ static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_xy); } /** @@ -188,7 +253,8 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlin */ static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_z); } /** @@ -198,7 +264,8 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink */ static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_yaw); } /** @@ -209,12 +276,5 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavli */ static inline void mavlink_msg_control_status_decode(const mavlink_message_t* msg, mavlink_control_status_t* control_status) { - control_status->position_fix = mavlink_msg_control_status_get_position_fix(msg); - control_status->vision_fix = mavlink_msg_control_status_get_vision_fix(msg); - control_status->gps_fix = mavlink_msg_control_status_get_gps_fix(msg); - control_status->ahrs_health = mavlink_msg_control_status_get_ahrs_health(msg); - control_status->control_att = mavlink_msg_control_status_get_control_att(msg); - control_status->control_pos_xy = mavlink_msg_control_status_get_control_pos_xy(msg); - control_status->control_pos_z = mavlink_msg_control_status_get_control_pos_z(msg); - control_status->control_pos_yaw = mavlink_msg_control_status_get_control_pos_yaw(msg); + memcpy( control_status, msg->payload, sizeof(mavlink_control_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index e98be847ad..9924063356 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -1,6 +1,8 @@ // MESSAGE DEBUG PACKING #define MAVLINK_MSG_ID_DEBUG 255 +#define MAVLINK_MSG_ID_DEBUG_LEN 5 +#define MAVLINK_MSG_255_LEN 5 typedef struct __mavlink_debug_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_debug_t } mavlink_debug_t; - - /** * @brief Pack a debug message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_debug_t */ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t ind, float value) { - uint16_t i = 0; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - i += put_uint8_t_by_index(ind, i, msg->payload); // index of debug variable - i += put_float_by_index(value, i, msg->payload); // DEBUG value + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone */ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t ind, float value) { - uint16_t i = 0; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - i += put_uint8_t_by_index(ind, i, msg->payload); // index of debug variable - i += put_float_by_index(value, i, msg->payload); // DEBUG value + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo * @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_message_t msg; - mavlink_msg_debug_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, ind, value); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_debug_t *p = (mavlink_debug_t *)&msg.payload[0]; + + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DEBUG_LEN; + msg.msgid = MAVLINK_MSG_ID_DEBUG; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) +{ + mavlink_header_t hdr; + mavlink_debug_t payload; + uint16_t checksum; + mavlink_debug_t *p = &payload; + + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DEBUG_LEN; + hdr.msgid = MAVLINK_MSG_ID_DEBUG; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, f */ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; + return (uint8_t)(p->ind); } /** @@ -102,12 +150,8 @@ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; + return (float)(p->value); } /** @@ -118,6 +162,5 @@ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) */ static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) { - debug->ind = mavlink_msg_debug_get_ind(msg); - debug->value = mavlink_msg_debug_get_value(msg); + memcpy( debug, msg->payload, sizeof(mavlink_debug_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 6ea9e0558b..b31289ac84 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -1,6 +1,8 @@ // MESSAGE DEBUG_VECT PACKING #define MAVLINK_MSG_ID_DEBUG_VECT 251 +#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 +#define MAVLINK_MSG_251_LEN 30 typedef struct __mavlink_debug_vect_t { @@ -11,10 +13,8 @@ typedef struct __mavlink_debug_vect_t float z; ///< z } mavlink_debug_vect_t; - #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 - /** * @brief Pack a debug_vect message * @param system_id ID of this system @@ -30,16 +30,16 @@ typedef struct __mavlink_debug_vect_t */ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp - i += put_float_by_index(x, i, msg->payload); // x - i += put_float_by_index(y, i, msg->payload); // y - i += put_float_by_index(z, i, msg->payload); // z + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } /** @@ -57,16 +57,16 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp - i += put_float_by_index(x, i, msg->payload); // x - i += put_float_by_index(y, i, msg->payload); // y - i += put_float_by_index(z, i, msg->payload); // z + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } /** @@ -93,12 +93,65 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param z 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_message_t msg; - mavlink_msg_debug_vect_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, usec, x, y, z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; + msg.msgid = MAVLINK_MSG_ID_DEBUG_VECT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_debug_vect_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; + hdr.msgid = MAVLINK_MSG_ID_DEBUG_VECT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -109,11 +162,12 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha * * @return Name */ -static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char* name) { + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -123,16 +177,8 @@ static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* */ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(char)*10)[0]; - r.b[6] = (msg->payload+sizeof(char)*10)[1]; - r.b[5] = (msg->payload+sizeof(char)*10)[2]; - r.b[4] = (msg->payload+sizeof(char)*10)[3]; - r.b[3] = (msg->payload+sizeof(char)*10)[4]; - r.b[2] = (msg->payload+sizeof(char)*10)[5]; - r.b[1] = (msg->payload+sizeof(char)*10)[6]; - r.b[0] = (msg->payload+sizeof(char)*10)[7]; - return (uint64_t)r.ll; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -142,12 +188,8 @@ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* */ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -157,12 +199,8 @@ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -172,12 +210,8 @@ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -188,9 +222,5 @@ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) */ static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect) { - mavlink_msg_debug_vect_get_name(msg, debug_vect->name); - debug_vect->usec = mavlink_msg_debug_vect_get_usec(msg); - debug_vect->x = mavlink_msg_debug_vect_get_x(msg); - debug_vect->y = mavlink_msg_debug_vect_get_y(msg); - debug_vect->z = mavlink_msg_debug_vect_get_z(msg); + memcpy( debug_vect, msg->payload, sizeof(mavlink_debug_vect_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index 9d27f7714b..bb5112eba5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -1,6 +1,8 @@ // MESSAGE GLOBAL_POSITION PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION 33 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 +#define MAVLINK_MSG_33_LEN 32 typedef struct __mavlink_global_position_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_global_position_t } mavlink_global_position_t; - - /** * @brief Pack a global_position message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_global_position_t */ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees - i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters - i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) - i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) - i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees - i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters - i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) - i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) - i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_global_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, lat, lon, alt, vx, vy, vz); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; + msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_global_position_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; + hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_messag */ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vx); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vy); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vz); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* */ static inline void mavlink_msg_global_position_decode(const mavlink_message_t* msg, mavlink_global_position_t* global_position) { - global_position->usec = mavlink_msg_global_position_get_usec(msg); - global_position->lat = mavlink_msg_global_position_get_lat(msg); - global_position->lon = mavlink_msg_global_position_get_lon(msg); - global_position->alt = mavlink_msg_global_position_get_alt(msg); - global_position->vx = mavlink_msg_global_position_get_vx(msg); - global_position->vy = mavlink_msg_global_position_get_vy(msg); - global_position->vz = mavlink_msg_global_position_get_vz(msg); + memcpy( global_position, msg->payload, sizeof(mavlink_global_position_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h index 1b3277a357..fb11a417ef 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -1,6 +1,8 @@ // MESSAGE GLOBAL_POSITION_INT PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 18 +#define MAVLINK_MSG_73_LEN 18 typedef struct __mavlink_global_position_int_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_global_position_int_t } mavlink_global_position_int_t; - - /** * @brief Pack a global_position_int message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_global_position_int_t */ 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 i = 0; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + 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 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u */ 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 i = 0; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + 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 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } /** @@ -98,12 +98,67 @@ 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 */ #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) { mavlink_message_t msg; - mavlink_msg_global_position_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, lat, lon, alt, vx, vy, vz); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; + msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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) +{ + mavlink_header_t hdr; + mavlink_global_position_int_t payload; + uint16_t checksum; + mavlink_global_position_int_t *p = &payload; + + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + 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 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, */ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (int32_t)r.i; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->lat); } /** @@ -131,12 +182,8 @@ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->lon); } /** @@ -146,12 +193,8 @@ 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) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->alt); } /** @@ -161,10 +204,8 @@ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_mess */ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - return (int16_t)r.s; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vx); } /** @@ -174,10 +215,8 @@ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vy); } /** @@ -187,10 +226,8 @@ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vz); } /** @@ -201,10 +238,5 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa */ static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) { - global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg); - global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg); - global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg); - global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg); - global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg); - global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); + memcpy( global_position_int, msg->payload, sizeof(mavlink_global_position_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h index 1562528541..8834ee6ce9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h @@ -1,6 +1,8 @@ // MESSAGE GPS_LOCAL_ORIGIN_SET PACKING #define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET 49 +#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN 12 +#define MAVLINK_MSG_49_LEN 12 typedef struct __mavlink_gps_local_origin_set_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_gps_local_origin_set_t } mavlink_gps_local_origin_set_t; - - /** * @brief Pack a gps_local_origin_set message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_gps_local_origin_set_t */ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - i += put_int32_t_by_index(latitude, i, msg->payload); // Latitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // Longitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // Altitude(WGS84), expressed as * 1000 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_gps_local_origin_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - i += put_int32_t_by_index(latitude, i, msg->payload); // Latitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // Longitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // Altitude(WGS84), expressed as * 1000 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id * @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_message_t msg; - mavlink_msg_gps_local_origin_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, latitude, longitude, altitude); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_gps_local_origin_set_t *p = &payload; + + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,12 +147,8 @@ static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, */ static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (int32_t)r.i; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->latitude); } /** @@ -113,12 +158,8 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(const mavlin */ static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->longitude); } /** @@ -128,12 +169,8 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavli */ static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->altitude); } /** @@ -144,7 +181,5 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlin */ static inline void mavlink_msg_gps_local_origin_set_decode(const mavlink_message_t* msg, mavlink_gps_local_origin_set_t* gps_local_origin_set) { - gps_local_origin_set->latitude = mavlink_msg_gps_local_origin_set_get_latitude(msg); - gps_local_origin_set->longitude = mavlink_msg_gps_local_origin_set_get_longitude(msg); - gps_local_origin_set->altitude = mavlink_msg_gps_local_origin_set_get_altitude(msg); + memcpy( gps_local_origin_set, msg->payload, sizeof(mavlink_gps_local_origin_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 52d33a23b2..ecaeb8b849 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -1,6 +1,8 @@ // MESSAGE GPS_RAW PACKING #define MAVLINK_MSG_ID_GPS_RAW 32 +#define MAVLINK_MSG_ID_GPS_RAW_LEN 37 +#define MAVLINK_MSG_32_LEN 37 typedef struct __mavlink_gps_raw_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_gps_raw_t } mavlink_gps_raw_t; - - /** * @brief Pack a gps_raw message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_gps_raw_t */ 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) { - uint16_t i = 0; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees - i += put_float_by_index(alt, i, msg->payload); // Altitude in meters - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo */ 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) { - uint16_t i = 0; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees - i += put_float_by_index(alt, i, msg->payload); // Altitude in meters - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_LEN); } /** @@ -116,12 +116,73 @@ 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 */ #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) { mavlink_message_t msg; - mavlink_msg_gps_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_RAW_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_RAW; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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) +{ + mavlink_header_t hdr; + mavlink_gps_raw_t payload; + uint16_t checksum; + mavlink_gps_raw_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,16 +195,8 @@ static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t use */ static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -153,7 +206,8 @@ static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint8_t)(p->fix_type); } /** @@ -163,12 +217,8 @@ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* */ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -178,12 +228,8 @@ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -193,12 +239,8 @@ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -208,12 +250,8 @@ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->eph); } /** @@ -223,12 +261,8 @@ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->epv); } /** @@ -238,12 +272,8 @@ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->v); } /** @@ -253,12 +283,8 @@ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->hdg); } /** @@ -269,13 +295,5 @@ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) */ static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavlink_gps_raw_t* gps_raw) { - gps_raw->usec = mavlink_msg_gps_raw_get_usec(msg); - gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); - gps_raw->lat = mavlink_msg_gps_raw_get_lat(msg); - gps_raw->lon = mavlink_msg_gps_raw_get_lon(msg); - gps_raw->alt = mavlink_msg_gps_raw_get_alt(msg); - gps_raw->eph = mavlink_msg_gps_raw_get_eph(msg); - gps_raw->epv = mavlink_msg_gps_raw_get_epv(msg); - gps_raw->v = mavlink_msg_gps_raw_get_v(msg); - gps_raw->hdg = mavlink_msg_gps_raw_get_hdg(msg); + memcpy( gps_raw, msg->payload, sizeof(mavlink_gps_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h index b49498febc..9fc5360b1f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -1,6 +1,8 @@ // MESSAGE GPS_RAW_INT PACKING #define MAVLINK_MSG_ID_GPS_RAW_INT 25 +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 37 +#define MAVLINK_MSG_25_LEN 37 typedef struct __mavlink_gps_raw_int_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_gps_raw_int_t } mavlink_gps_raw_int_t; - - /** * @brief Pack a gps_raw_int message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_gps_raw_int_t */ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) { - uint16_t i = 0; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude in 1E7 degrees - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude in 1E7 degrees - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in 1E3 meters (millimeters) - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed (m/s) - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) { - uint16_t i = 0; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude in 1E7 degrees - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude in 1E7 degrees - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in 1E3 meters (millimeters) - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed (m/s) - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param hdg Compass heading in degrees, 0..360 degrees */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) { mavlink_message_t msg; - mavlink_msg_gps_raw_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg.payload[0]; + + 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) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) +{ + mavlink_header_t hdr; + mavlink_gps_raw_int_t payload; + uint16_t checksum; + mavlink_gps_raw_int_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,16 +195,8 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_gps_raw_int_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -153,7 +206,8 @@ static inline uint64_t mavlink_msg_gps_raw_int_get_usec(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint8_t)(p->fix_type); } /** @@ -163,12 +217,8 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message */ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->lat); } /** @@ -178,12 +228,8 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->lon); } /** @@ -193,12 +239,8 @@ 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) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->alt); } /** @@ -208,12 +250,8 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m */ static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->eph); } /** @@ -223,12 +261,8 @@ static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg */ static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->epv); } /** @@ -238,12 +272,8 @@ static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg */ static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->v); } /** @@ -253,12 +283,8 @@ static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->hdg); } /** @@ -269,13 +295,5 @@ static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg */ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int) { - gps_raw_int->usec = mavlink_msg_gps_raw_int_get_usec(msg); - gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); - gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg); - gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg); - gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg); - gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg); - gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg); - gps_raw_int->v = mavlink_msg_gps_raw_int_get_v(msg); - gps_raw_int->hdg = mavlink_msg_gps_raw_int_get_hdg(msg); + memcpy( gps_raw_int, msg->payload, sizeof(mavlink_gps_raw_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h index 47ddc2e23b..f7aaf96da1 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,6 +1,8 @@ // MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING #define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 +#define MAVLINK_MSG_48_LEN 14 typedef struct __mavlink_gps_set_global_origin_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_gps_set_global_origin_t } mavlink_gps_set_global_origin_t; - - /** * @brief Pack a gps_set_global_origin message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_gps_set_global_origin_t */ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_int32_t_by_index(latitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // global position * 1000 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_int32_t_by_index(latitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // global position * 1000 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i * @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_message_t msg; - mavlink_msg_gps_set_global_origin_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, latitude, longitude, altitude); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_gps_set_global_origin_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -120,7 +174,8 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const */ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -130,12 +185,8 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(con */ static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->latitude); } /** @@ -145,12 +196,8 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavli */ static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->longitude); } /** @@ -160,12 +207,8 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavl */ static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->altitude); } /** @@ -176,9 +219,5 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavli */ static inline void mavlink_msg_gps_set_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_set_global_origin_t* gps_set_global_origin) { - gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg); - gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg); - gps_set_global_origin->latitude = mavlink_msg_gps_set_global_origin_get_latitude(msg); - gps_set_global_origin->longitude = mavlink_msg_gps_set_global_origin_get_longitude(msg); - gps_set_global_origin->altitude = mavlink_msg_gps_set_global_origin_get_altitude(msg); + memcpy( gps_set_global_origin, msg->payload, sizeof(mavlink_gps_set_global_origin_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index fd04a9a262..484de2c012 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -1,25 +1,25 @@ // MESSAGE GPS_STATUS PACKING #define MAVLINK_MSG_ID_GPS_STATUS 27 +#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 +#define MAVLINK_MSG_27_LEN 101 typedef struct __mavlink_gps_status_t { uint8_t satellites_visible; ///< Number of satellites visible - int8_t satellite_prn[20]; ///< Global satellite ID - int8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization - int8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite - int8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. - int8_t satellite_snr[20]; ///< Signal to noise ratio of satellite + uint8_t satellite_prn[20]; ///< Global satellite ID + uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization + uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite } mavlink_gps_status_t; - #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_USED_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_ELEVATION_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_AZIMUTH_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_SNR_LEN 20 - /** * @brief Pack a gps_status message * @param system_id ID of this system @@ -34,19 +34,19 @@ typedef struct __mavlink_gps_status_t * @param satellite_snr Signal to noise ratio of satellite * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) +static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - uint16_t i = 0; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - i += put_uint8_t_by_index(satellites_visible, i, msg->payload); // Number of satellites visible - i += put_array_by_index(satellite_prn, 20, i, msg->payload); // Global satellite ID - i += put_array_by_index(satellite_used, 20, i, msg->payload); // 0: Satellite not used, 1: used for localization - i += put_array_by_index(satellite_elevation, 20, i, msg->payload); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - i += put_array_by_index(satellite_azimuth, 20, i, msg->payload); // Direction of satellite, 0: 0 deg, 255: 360 deg. - i += put_array_by_index(satellite_snr, 20, i, msg->payload); // Signal to noise ratio of satellite + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); } /** @@ -63,19 +63,19 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co * @param satellite_snr Signal to noise ratio of satellite * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) +static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - uint16_t i = 0; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - i += put_uint8_t_by_index(satellites_visible, i, msg->payload); // Number of satellites visible - i += put_array_by_index(satellite_prn, 20, i, msg->payload); // Global satellite ID - i += put_array_by_index(satellite_used, 20, i, msg->payload); // 0: Satellite not used, 1: used for localization - i += put_array_by_index(satellite_elevation, 20, i, msg->payload); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - i += put_array_by_index(satellite_azimuth, 20, i, msg->payload); // Direction of satellite, 0: 0 deg, 255: 360 deg. - i += put_array_by_index(satellite_snr, 20, i, msg->payload); // Signal to noise ratio of satellite + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); } /** @@ -103,12 +103,67 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_snr Signal to noise ratio of satellite */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) +static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { mavlink_message_t msg; - mavlink_msg_gps_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_gps_status_t *p = &payload; + + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -121,7 +176,8 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; + return (uint8_t)(p->satellites_visible); } /** @@ -129,11 +185,12 @@ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlin * * @return Global satellite ID */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, uint8_t* satellite_prn) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t), 20); - return 20; + memcpy(satellite_prn, p->satellite_prn, sizeof(p->satellite_prn)); + return sizeof(p->satellite_prn); } /** @@ -141,11 +198,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_me * * @return 0: Satellite not used, 1: used for localization */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, uint8_t* satellite_used) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20, 20); - return 20; + memcpy(satellite_used, p->satellite_used, sizeof(p->satellite_used)); + return sizeof(p->satellite_used); } /** @@ -153,11 +211,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_m * * @return Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, uint8_t* satellite_elevation) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20, 20); - return 20; + memcpy(satellite_elevation, p->satellite_elevation, sizeof(p->satellite_elevation)); + return sizeof(p->satellite_elevation); } /** @@ -165,11 +224,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavl * * @return Direction of satellite, 0: 0 deg, 255: 360 deg. */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, uint8_t* satellite_azimuth) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20, 20); - return 20; + memcpy(satellite_azimuth, p->satellite_azimuth, sizeof(p->satellite_azimuth)); + return sizeof(p->satellite_azimuth); } /** @@ -177,11 +237,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlin * * @return Signal to noise ratio of satellite */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, uint8_t* satellite_snr) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20+20, 20); - return 20; + memcpy(satellite_snr, p->satellite_snr, sizeof(p->satellite_snr)); + return sizeof(p->satellite_snr); } /** @@ -192,10 +253,5 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_me */ static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status) { - gps_status->satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg); - mavlink_msg_gps_status_get_satellite_prn(msg, gps_status->satellite_prn); - mavlink_msg_gps_status_get_satellite_used(msg, gps_status->satellite_used); - mavlink_msg_gps_status_get_satellite_elevation(msg, gps_status->satellite_elevation); - mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth); - mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr); + memcpy( gps_status, msg->payload, sizeof(mavlink_gps_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 0e5c4db5ca..fea5381e9c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_0_LEN 3 typedef struct __mavlink_heartbeat_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_heartbeat_t } mavlink_heartbeat_t; - - /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -24,14 +24,14 @@ typedef struct __mavlink_heartbeat_t */ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message(msg, system_id, component_id, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -46,14 +46,14 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -77,12 +77,61 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) +{ + mavlink_header_t hdr; + mavlink_heartbeat_t payload; + uint16_t checksum; + mavlink_heartbeat_t *p = &payload; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -95,7 +144,8 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -105,7 +155,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->autopilot); } /** @@ -115,7 +166,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->mavlink_version); } /** @@ -126,7 +178,5 @@ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_me */ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) { - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); + memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index c9cb453c0e..2fbe01f2ab 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION 31 +#define MAVLINK_MSG_ID_LOCAL_POSITION_LEN 32 +#define MAVLINK_MSG_31_LEN 32 typedef struct __mavlink_local_position_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_local_position_t } mavlink_local_position_t; - - /** * @brief Pack a local_position message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_local_position_t */ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint * @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_message_t msg; - mavlink_msg_local_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, vx, vy, vz); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; + msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_local_position_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint6 */ static inline uint64_t mavlink_msg_local_position_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_local_position_get_usec(const mavlink_message */ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vx); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vy); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vz); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* m */ static inline void mavlink_msg_local_position_decode(const mavlink_message_t* msg, mavlink_local_position_t* local_position) { - local_position->usec = mavlink_msg_local_position_get_usec(msg); - local_position->x = mavlink_msg_local_position_get_x(msg); - local_position->y = mavlink_msg_local_position_get_y(msg); - local_position->z = mavlink_msg_local_position_get_z(msg); - local_position->vx = mavlink_msg_local_position_get_vx(msg); - local_position->vy = mavlink_msg_local_position_get_vy(msg); - local_position->vz = mavlink_msg_local_position_get_vz(msg); + memcpy( local_position, msg->payload, sizeof(mavlink_local_position_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h index fcf4733476..2f6ceb808c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION_SETPOINT PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 16 +#define MAVLINK_MSG_51_LEN 16 typedef struct __mavlink_local_position_setpoint_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_local_position_setpoint_t } mavlink_local_position_setpoint_t; - - /** * @brief Pack a local_position_setpoint message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_local_position_setpoint_t */ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system * @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_message_t msg; - mavlink_msg_local_position_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; + msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_local_position_setpoint_t *p = &payload; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +155,8 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch */ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -119,12 +166,8 @@ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -134,12 +177,8 @@ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -149,12 +188,8 @@ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -165,8 +200,5 @@ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_me */ static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_t* local_position_setpoint) { - local_position_setpoint->x = mavlink_msg_local_position_setpoint_get_x(msg); - local_position_setpoint->y = mavlink_msg_local_position_setpoint_get_y(msg); - local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg); - local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg); + memcpy( local_position_setpoint, msg->payload, sizeof(mavlink_local_position_setpoint_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h index f3383287eb..9f34862b8e 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,6 +1,8 @@ // MESSAGE LOCAL_POSITION_SETPOINT_SET PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN 18 +#define MAVLINK_MSG_50_LEN 18 typedef struct __mavlink_local_position_setpoint_set_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_local_position_setpoint_set_t } mavlink_local_position_setpoint_set_t; - - /** * @brief Pack a local_position_setpoint_set message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_local_position_setpoint_set_t */ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst */ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy * @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_message_t msg; - mavlink_msg_local_position_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_local_position_setpoint_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +171,8 @@ static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -126,7 +182,8 @@ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system( */ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -136,12 +193,8 @@ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_compone */ static inline float mavlink_msg_local_position_setpoint_set_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -151,12 +204,8 @@ static inline float mavlink_msg_local_position_setpoint_set_get_x(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -166,12 +215,8 @@ static inline float mavlink_msg_local_position_setpoint_set_get_y(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -181,12 +226,8 @@ static inline float mavlink_msg_local_position_setpoint_set_get_z(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -197,10 +238,5 @@ static inline float mavlink_msg_local_position_setpoint_set_get_yaw(const mavlin */ static inline void mavlink_msg_local_position_setpoint_set_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_set_t* local_position_setpoint_set) { - local_position_setpoint_set->target_system = mavlink_msg_local_position_setpoint_set_get_target_system(msg); - local_position_setpoint_set->target_component = mavlink_msg_local_position_setpoint_set_get_target_component(msg); - local_position_setpoint_set->x = mavlink_msg_local_position_setpoint_set_get_x(msg); - local_position_setpoint_set->y = mavlink_msg_local_position_setpoint_set_get_y(msg); - local_position_setpoint_set->z = mavlink_msg_local_position_setpoint_set_get_z(msg); - local_position_setpoint_set->yaw = mavlink_msg_local_position_setpoint_set_get_yaw(msg); + memcpy( local_position_setpoint_set, msg->payload, sizeof(mavlink_local_position_setpoint_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index deec098f63..d3aede95be 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -1,6 +1,8 @@ // MESSAGE MANUAL_CONTROL PACKING #define MAVLINK_MSG_ID_MANUAL_CONTROL 69 +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 21 +#define MAVLINK_MSG_69_LEN 21 typedef struct __mavlink_manual_control_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_manual_control_t } mavlink_manual_control_t; - - /** * @brief Pack a manual_control message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_manual_control_t */ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint * @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_message_t msg; - mavlink_msg_manual_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_manual_control_t *p = &payload; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +195,8 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -144,12 +206,8 @@ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_messag */ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -159,12 +217,8 @@ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -174,12 +228,8 @@ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t */ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -189,12 +239,8 @@ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->thrust); } /** @@ -204,7 +250,8 @@ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_ */ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->roll_manual); } /** @@ -214,7 +261,8 @@ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_m */ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->pitch_manual); } /** @@ -224,7 +272,8 @@ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_ */ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->yaw_manual); } /** @@ -234,7 +283,8 @@ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_me */ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->thrust_manual); } /** @@ -245,13 +295,5 @@ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink */ static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control) { - manual_control->target = mavlink_msg_manual_control_get_target(msg); - manual_control->roll = mavlink_msg_manual_control_get_roll(msg); - manual_control->pitch = mavlink_msg_manual_control_get_pitch(msg); - manual_control->yaw = mavlink_msg_manual_control_get_yaw(msg); - manual_control->thrust = mavlink_msg_manual_control_get_thrust(msg); - manual_control->roll_manual = mavlink_msg_manual_control_get_roll_manual(msg); - manual_control->pitch_manual = mavlink_msg_manual_control_get_pitch_manual(msg); - manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg); - manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg); + memcpy( manual_control, msg->payload, sizeof(mavlink_manual_control_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h index aaff215ce8..6f4fb8830f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -1,6 +1,8 @@ // MESSAGE NAMED_VALUE_FLOAT PACKING #define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 252 +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 14 +#define MAVLINK_MSG_252_LEN 14 typedef struct __mavlink_named_value_float_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_named_value_float_t float value; ///< Floating point value } mavlink_named_value_float_t; - #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 - /** * @brief Pack a named_value_float message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_named_value_float_t */ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, float value) { - uint16_t i = 0; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_float_by_index(value, i, msg->payload); // Floating point value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, float value) { - uint16_t i = 0; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_float_by_index(value, i, msg->payload); // Floating point value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param value Floating point 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_message_t msg; - mavlink_msg_named_value_float_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, value); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg.payload[0]; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; + msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_named_value_float_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -91,11 +138,12 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, co * * @return Name of the debug variable */ -static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char* name) { + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -105,12 +153,8 @@ static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_mess */ static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10)[0]; - r.b[2] = (msg->payload+sizeof(char)*10)[1]; - r.b[1] = (msg->payload+sizeof(char)*10)[2]; - r.b[0] = (msg->payload+sizeof(char)*10)[3]; - return (float)r.f; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; + return (float)(p->value); } /** @@ -121,6 +165,5 @@ static inline float mavlink_msg_named_value_float_get_value(const mavlink_messag */ static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* msg, mavlink_named_value_float_t* named_value_float) { - mavlink_msg_named_value_float_get_name(msg, named_value_float->name); - named_value_float->value = mavlink_msg_named_value_float_get_value(msg); + memcpy( named_value_float, msg->payload, sizeof(mavlink_named_value_float_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h index 92bf5fcb2a..a793614f5e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -1,6 +1,8 @@ // MESSAGE NAMED_VALUE_INT PACKING #define MAVLINK_MSG_ID_NAMED_VALUE_INT 253 +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 14 +#define MAVLINK_MSG_253_LEN 14 typedef struct __mavlink_named_value_int_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_named_value_int_t int32_t value; ///< Signed integer value } mavlink_named_value_int_t; - #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 - /** * @brief Pack a named_value_int message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_named_value_int_t */ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, int32_t value) { - uint16_t i = 0; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_int32_t_by_index(value, i, msg->payload); // Signed integer value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, int32_t value) { - uint16_t i = 0; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_int32_t_by_index(value, i, msg->payload); // Signed integer value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param value Signed integer 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_message_t msg; - mavlink_msg_named_value_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, value); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg.payload[0]; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; + msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_named_value_int_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -91,11 +138,12 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, cons * * @return Name of the debug variable */ -static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char* name) { + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -105,12 +153,8 @@ static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_messag */ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10)[0]; - r.b[2] = (msg->payload+sizeof(char)*10)[1]; - r.b[1] = (msg->payload+sizeof(char)*10)[2]; - r.b[0] = (msg->payload+sizeof(char)*10)[3]; - return (int32_t)r.i; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; + return (int32_t)(p->value); } /** @@ -121,6 +165,5 @@ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_messag */ static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* msg, mavlink_named_value_int_t* named_value_int) { - mavlink_msg_named_value_int_get_name(msg, named_value_int->name); - named_value_int->value = mavlink_msg_named_value_int_get_value(msg); + memcpy( named_value_int, msg->payload, sizeof(mavlink_named_value_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h index 8d0b81eb87..f19d85b0a1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE NAV_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 +#define MAVLINK_MSG_62_LEN 26 typedef struct __mavlink_nav_controller_output_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_nav_controller_output_t } mavlink_nav_controller_output_t; - - /** * @brief Pack a nav_controller_output message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_nav_controller_output_t */ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { - uint16_t i = 0; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { - uint16_t i = 0; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i * @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_message_t msg; - mavlink_msg_nav_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; + msg.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_nav_controller_output_t *p = &payload; + + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,12 +187,8 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan */ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->nav_roll); } /** @@ -143,12 +198,8 @@ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink */ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->nav_pitch); } /** @@ -158,10 +209,8 @@ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlin */ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (int16_t)(p->nav_bearing); } /** @@ -171,10 +220,8 @@ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const ma */ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (int16_t)(p->target_bearing); } /** @@ -184,10 +231,8 @@ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const */ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (uint16_t)r.s; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (uint16_t)(p->wp_dist); } /** @@ -197,12 +242,8 @@ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavli */ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->alt_error); } /** @@ -212,12 +253,8 @@ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlin */ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->aspd_error); } /** @@ -227,12 +264,8 @@ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavli */ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->xtrack_error); } /** @@ -243,12 +276,5 @@ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mav */ static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output) { - nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); - nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); - nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); - nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); - nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); - nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); - nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); - nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); + memcpy( nav_controller_output, msg->payload, sizeof(mavlink_nav_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h index e4912fb591..d53cfe7d8b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -1,6 +1,8 @@ // MESSAGE PARAM_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_21_LEN 2 typedef struct __mavlink_param_request_list_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_param_request_list_t } mavlink_param_request_list_t; - - /** * @brief Pack a param_request_list message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_param_request_list_t */ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t 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_message_t msg; - mavlink_msg_param_request_list_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_param_request_list_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +150,8 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +162,5 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const */ static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) { - param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); - param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); + memcpy( param_request_list, msg->payload, sizeof(mavlink_param_request_list_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h index 2cd9d2932e..a60179da97 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -1,19 +1,19 @@ // MESSAGE PARAM_REQUEST_READ PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 19 +#define MAVLINK_MSG_20_LEN 19 typedef struct __mavlink_param_request_read_t { uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - int8_t param_id[15]; ///< Onboard parameter id + char param_id[15]; ///< Onboard parameter id int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier } mavlink_param_request_read_t; - #define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15 - /** * @brief Pack a param_request_read message * @param system_id ID of this system @@ -26,17 +26,17 @@ typedef struct __mavlink_param_request_read_t * @param param_index Parameter index. Send -1 to use the param ID field as identifier * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const int8_t* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { - uint16_t i = 0; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_int16_t_by_index(param_index, i, msg->payload); // Parameter index. Send -1 to use the param ID field as identifier + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @param param_index Parameter index. Send -1 to use the param ID field as identifier * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const int8_t* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { - uint16_t i = 0; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_int16_t_by_index(param_index, i, msg->payload); // Parameter index. Send -1 to use the param ID field as identifier + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t* param_id, int16_t param_index) +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { mavlink_message_t msg; - mavlink_msg_param_request_read_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, param_id, param_index); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg.payload[0]; + + 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[15]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_param_request_read_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +167,8 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -123,11 +176,12 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char* param_id) { + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -137,10 +191,8 @@ static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink */ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[1]; - return (int16_t)r.s; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (int16_t)(p->param_index); } /** @@ -151,8 +203,5 @@ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavli */ static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read) { - param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); - param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); - mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); - param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); + memcpy( param_request_read, msg->payload, sizeof(mavlink_param_request_read_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index de1c314022..476c980957 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -1,19 +1,19 @@ // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 +#define MAVLINK_MSG_ID_PARAM_SET_LEN 21 +#define MAVLINK_MSG_23_LEN 21 typedef struct __mavlink_param_set_t { uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - int8_t param_id[15]; ///< Onboard parameter id + char param_id[15]; ///< Onboard parameter id float param_value; ///< Onboard parameter value } mavlink_param_set_t; - #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15 - /** * @brief Pack a param_set message * @param system_id ID of this system @@ -26,17 +26,17 @@ typedef struct __mavlink_param_set_t * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const int8_t* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { - uint16_t i = 0; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const int8_t* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { - uint16_t i = 0; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_value Onboard parameter value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t* param_id, float param_value) +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { mavlink_message_t msg; - mavlink_msg_param_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, param_id, param_value); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg.payload[0]; + + 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[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +{ + mavlink_header_t hdr; + mavlink_param_set_t payload; + uint16_t checksum; + mavlink_param_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta */ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +167,8 @@ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_mess */ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -123,11 +176,12 @@ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_m * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char* param_id) { + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -137,12 +191,8 @@ static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_ */ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[3]; - return (float)r.f; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (float)(p->param_value); } /** @@ -153,8 +203,5 @@ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_ */ static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) { - param_set->target_system = mavlink_msg_param_set_get_target_system(msg); - param_set->target_component = mavlink_msg_param_set_get_target_component(msg); - mavlink_msg_param_set_get_param_id(msg, param_set->param_id); - param_set->param_value = mavlink_msg_param_set_get_param_value(msg); + memcpy( param_set, msg->payload, sizeof(mavlink_param_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 239c96c42f..3834408454 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -1,19 +1,19 @@ // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 23 +#define MAVLINK_MSG_22_LEN 23 typedef struct __mavlink_param_value_t { - int8_t param_id[15]; ///< Onboard parameter id + char param_id[15]; ///< Onboard parameter id float param_value; ///< Onboard parameter value uint16_t param_count; ///< Total number of onboard parameters uint16_t param_index; ///< Index of this onboard parameter } mavlink_param_value_t; - #define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 15 - /** * @brief Pack a param_value message * @param system_id ID of this system @@ -26,17 +26,17 @@ typedef struct __mavlink_param_value_t * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - uint16_t i = 0; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - i += put_uint16_t_by_index(param_count, i, msg->payload); // Total number of onboard parameters - i += put_uint16_t_by_index(param_index, i, msg->payload); // Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - uint16_t i = 0; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - i += put_uint16_t_by_index(param_count, i, msg->payload); // Total number of onboard parameters - i += put_uint16_t_by_index(param_index, i, msg->payload); // Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_index Index of this onboard parameter */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_message_t msg; - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, param_id, param_value, param_count, param_index); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg.payload[0]; + + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) +{ + mavlink_header_t hdr; + mavlink_param_value_t payload; + uint16_t checksum; + mavlink_param_value_t *p = &payload; + + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -103,11 +154,12 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const in * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char* param_id) { + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -117,12 +169,8 @@ static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_messag */ static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+15)[0]; - r.b[2] = (msg->payload+15)[1]; - r.b[1] = (msg->payload+15)[2]; - r.b[0] = (msg->payload+15)[3]; - return (float)r.f; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (float)(p->param_value); } /** @@ -132,10 +180,8 @@ static inline float mavlink_msg_param_value_get_param_value(const mavlink_messag */ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+15+sizeof(float))[0]; - r.b[0] = (msg->payload+15+sizeof(float))[1]; - return (uint16_t)r.s; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (uint16_t)(p->param_count); } /** @@ -145,10 +191,8 @@ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_mes */ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+15+sizeof(float)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+15+sizeof(float)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (uint16_t)(p->param_index); } /** @@ -159,8 +203,5 @@ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_mes */ static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value) { - mavlink_msg_param_value_get_param_id(msg, param_value->param_id); - param_value->param_value = mavlink_msg_param_value_get_param_value(msg); - param_value->param_count = mavlink_msg_param_value_get_param_count(msg); - param_value->param_index = mavlink_msg_param_value_get_param_index(msg); + memcpy( param_value, msg->payload, sizeof(mavlink_param_value_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index 2a27b1e5cb..2e89aa2b0c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -1,6 +1,8 @@ // MESSAGE PING PACKING #define MAVLINK_MSG_ID_PING 3 +#define MAVLINK_MSG_ID_PING_LEN 14 +#define MAVLINK_MSG_3_LEN 14 typedef struct __mavlink_ping_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_ping_t } mavlink_ping_t; - - /** * @brief Pack a ping message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_ping_t */ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { - uint16_t i = 0; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - i += put_uint32_t_by_index(seq, i, msg->payload); // PING sequence - i += put_uint8_t_by_index(target_system, i, msg->payload); // 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - i += put_uint8_t_by_index(target_component, i, msg->payload); // 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - i += put_uint64_t_by_index(time, i, msg->payload); // Unix timestamp in microseconds + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen */ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { - uint16_t i = 0; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - i += put_uint32_t_by_index(seq, i, msg->payload); // PING sequence - i += put_uint8_t_by_index(target_system, i, msg->payload); // 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - i += put_uint8_t_by_index(target_component, i, msg->payload); // 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - i += put_uint64_t_by_index(time, i, msg->payload); // Unix timestamp in microseconds + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon * @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_message_t msg; - mavlink_msg_ping_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq, target_system, target_component, time); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_ping_t *p = (mavlink_ping_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PING_LEN; + msg.msgid = MAVLINK_MSG_ID_PING; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_ping_t *p = &payload; + + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PING_LEN; + hdr.msgid = MAVLINK_MSG_ID_PING; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +155,8 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, u */ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint32_t)(p->seq); } /** @@ -119,7 +166,8 @@ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) */ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint32_t))[0]; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -129,7 +177,8 @@ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t */ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint8_t))[0]; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -139,16 +188,8 @@ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_messag */ static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint64_t)(p->time); } /** @@ -159,8 +200,5 @@ static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) */ static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping) { - ping->seq = mavlink_msg_ping_get_seq(msg); - ping->target_system = mavlink_msg_ping_get_target_system(msg); - ping->target_component = mavlink_msg_ping_get_target_component(msg); - ping->time = mavlink_msg_ping_get_time(msg); + memcpy( ping, msg->payload, sizeof(mavlink_ping_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h index 389d9a2387..2935301ef4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT 61 +#define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN 5 +#define MAVLINK_MSG_61_LEN 5 typedef struct __mavlink_position_controller_output_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_position_controller_output_t } mavlink_position_controller_output_t; - - /** * @brief Pack a position_controller_output message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_position_controller_output_t */ static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { - uint16_t i = 0; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t syste */ static inline uint16_t mavlink_msg_position_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { - uint16_t i = 0; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t sys * @param yaw Position yaw: -128: -100%, 127: +100% */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { mavlink_message_t msg; - mavlink_msg_position_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg.payload[0]; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) +{ + mavlink_header_t hdr; + mavlink_position_controller_output_t payload; + uint16_t checksum; + mavlink_position_controller_output_t *p = &payload; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (uint8_t)(p->enabled); } /** @@ -120,7 +174,8 @@ static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const m */ static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->x); } /** @@ -130,7 +185,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_ */ static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->y); } /** @@ -140,7 +196,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_ */ static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->z); } /** @@ -150,7 +207,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_ */ static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->yaw); } /** @@ -161,9 +219,5 @@ static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlin */ static inline void mavlink_msg_position_controller_output_decode(const mavlink_message_t* msg, mavlink_position_controller_output_t* position_controller_output) { - position_controller_output->enabled = mavlink_msg_position_controller_output_get_enabled(msg); - position_controller_output->x = mavlink_msg_position_controller_output_get_x(msg); - position_controller_output->y = mavlink_msg_position_controller_output_get_y(msg); - position_controller_output->z = mavlink_msg_position_controller_output_get_z(msg); - position_controller_output->yaw = mavlink_msg_position_controller_output_get_yaw(msg); + memcpy( position_controller_output, msg->payload, sizeof(mavlink_position_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index d6e4da3dd4..7d9c425a62 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_TARGET PACKING #define MAVLINK_MSG_ID_POSITION_TARGET 63 +#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 +#define MAVLINK_MSG_63_LEN 16 typedef struct __mavlink_position_target_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_position_target_t } mavlink_position_target_t; - - /** * @brief Pack a position_target message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_position_target_t */ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_position_target_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_TARGET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_position_target_t *p = &payload; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_TARGET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +155,8 @@ static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, floa */ static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -119,12 +166,8 @@ static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -134,12 +177,8 @@ static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -149,12 +188,8 @@ static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -165,8 +200,5 @@ static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* */ static inline void mavlink_msg_position_target_decode(const mavlink_message_t* msg, mavlink_position_target_t* position_target) { - position_target->x = mavlink_msg_position_target_get_x(msg); - position_target->y = mavlink_msg_position_target_get_y(msg); - position_target->z = mavlink_msg_position_target_get_z(msg); - position_target->yaw = mavlink_msg_position_target_get_yaw(msg); + memcpy( position_target, msg->payload, sizeof(mavlink_position_target_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index 4ded0b7119..a815b1fa03 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -1,6 +1,8 @@ // MESSAGE RAW_IMU PACKING #define MAVLINK_MSG_ID_RAW_IMU 28 +#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_28_LEN 26 typedef struct __mavlink_raw_imu_t { @@ -17,8 +19,6 @@ typedef struct __mavlink_raw_imu_t } mavlink_raw_imu_t; - - /** * @brief Pack a raw_imu message * @param system_id ID of this system @@ -39,21 +39,21 @@ typedef struct __mavlink_raw_imu_t */ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (raw) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (raw) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (raw) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (raw) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (raw) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (raw) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (raw) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (raw) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (raw) - - return mavlink_finalize_message(msg, system_id, component_id, i); + 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); } /** @@ -76,21 +76,21 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (raw) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (raw) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (raw) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (raw) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (raw) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (raw) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (raw) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (raw) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (raw) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + 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); } /** @@ -122,12 +122,75 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com * @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_message_t msg; - mavlink_msg_raw_imu_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RAW_IMU_LEN; + msg.msgid = MAVLINK_MSG_ID_RAW_IMU; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_raw_imu_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_IMU_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_IMU; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -140,16 +203,8 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t use */ static inline uint64_t mavlink_msg_raw_imu_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -159,10 +214,8 @@ static inline uint64_t mavlink_msg_raw_imu_get_usec(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xacc); } /** @@ -172,10 +225,8 @@ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->yacc); } /** @@ -185,10 +236,8 @@ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zacc); } /** @@ -198,10 +247,8 @@ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xgyro); } /** @@ -211,10 +258,8 @@ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->ygyro); } /** @@ -224,10 +269,8 @@ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zgyro); } /** @@ -237,10 +280,8 @@ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xmag); } /** @@ -250,10 +291,8 @@ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->ymag); } /** @@ -263,10 +302,8 @@ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zmag); } /** @@ -277,14 +314,5 @@ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) */ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu) { - raw_imu->usec = mavlink_msg_raw_imu_get_usec(msg); - raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg); - raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg); - raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg); - raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg); - raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg); - raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg); - raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); - raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); - raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); + memcpy( raw_imu, msg->payload, sizeof(mavlink_raw_imu_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 8346ac6286..ccd6c59ac8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -1,6 +1,8 @@ // MESSAGE RAW_PRESSURE PACKING #define MAVLINK_MSG_ID_RAW_PRESSURE 29 +#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 +#define MAVLINK_MSG_29_LEN 16 typedef struct __mavlink_raw_pressure_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_raw_pressure_t } mavlink_raw_pressure_t; - - /** * @brief Pack a raw_pressure message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_raw_pressure_t */ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - uint16_t i = 0; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(press_abs, i, msg->payload); // Absolute pressure (raw) - i += put_int16_t_by_index(press_diff1, i, msg->payload); // Differential pressure 1 (raw) - i += put_int16_t_by_index(press_diff2, i, msg->payload); // Differential pressure 2 (raw) - i += put_int16_t_by_index(temperature, i, msg->payload); // Raw Temperature measurement (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - uint16_t i = 0; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(press_abs, i, msg->payload); // Absolute pressure (raw) - i += put_int16_t_by_index(press_diff1, i, msg->payload); // Differential pressure 1 (raw) - i += put_int16_t_by_index(press_diff2, i, msg->payload); // Differential pressure 2 (raw) - i += put_int16_t_by_index(temperature, i, msg->payload); // Raw Temperature measurement (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_raw_pressure_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, press_abs, press_diff1, press_diff2, temperature); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; + msg.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_raw_pressure_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,16 +163,8 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_raw_pressure_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -129,10 +174,8 @@ static inline uint64_t mavlink_msg_raw_pressure_get_usec(const mavlink_message_t */ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_abs); } /** @@ -142,10 +185,8 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_messa */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_diff1); } /** @@ -155,10 +196,8 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_diff2); } /** @@ -168,10 +207,8 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->temperature); } /** @@ -182,9 +219,5 @@ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_mes */ static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, mavlink_raw_pressure_t* raw_pressure) { - raw_pressure->usec = mavlink_msg_raw_pressure_get_usec(msg); - raw_pressure->press_abs = mavlink_msg_raw_pressure_get_press_abs(msg); - raw_pressure->press_diff1 = mavlink_msg_raw_pressure_get_press_diff1(msg); - raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg); - raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg); + memcpy( raw_pressure, msg->payload, sizeof(mavlink_raw_pressure_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h index 0443838bdc..7f7351d652 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -1,6 +1,8 @@ // MESSAGE RC_CHANNELS_RAW PACKING #define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 17 +#define MAVLINK_MSG_35_LEN 17 typedef struct __mavlink_rc_channels_raw_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_rc_channels_raw_t } mavlink_rc_channels_raw_t; - - /** * @brief Pack a rc_channels_raw message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_rc_channels_raw_t */ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds - i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds - i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds - i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds - i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds - i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds - i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds - i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds - i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds - i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds - i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds - i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds - i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds - i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds - i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_rc_channels_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg.payload[0]; + + 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% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; + msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_rc_channels_raw_t *p = &payload; + + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,10 +195,8 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan1_raw); } /** @@ -147,10 +206,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan2_raw); } /** @@ -160,10 +217,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan3_raw); } /** @@ -173,10 +228,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan4_raw); } /** @@ -186,10 +239,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan5_raw); } /** @@ -199,10 +250,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan6_raw); } /** @@ -212,10 +261,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan7_raw); } /** @@ -225,10 +272,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan8_raw); } /** @@ -238,7 +283,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_m */ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint8_t)(p->rssi); } /** @@ -249,13 +295,5 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message */ static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* msg, mavlink_rc_channels_raw_t* rc_channels_raw) { - rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); - rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); - rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); - rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); - rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); - rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); - rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); - rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); - rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); + memcpy( rc_channels_raw, msg->payload, sizeof(mavlink_rc_channels_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h index 526482b4e0..16bf4791e3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -1,6 +1,8 @@ // MESSAGE RC_CHANNELS_SCALED PACKING #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 17 +#define MAVLINK_MSG_36_LEN 17 typedef struct __mavlink_rc_channels_scaled_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_rc_channels_scaled_t } mavlink_rc_channels_scaled_t; - - /** * @brief Pack a rc_channels_scaled message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_rc_channels_scaled_t */ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - i += put_int16_t_by_index(chan1_scaled, i, msg->payload); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan2_scaled, i, msg->payload); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan3_scaled, i, msg->payload); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan4_scaled, i, msg->payload); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan5_scaled, i, msg->payload); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan6_scaled, i, msg->payload); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan7_scaled, i, msg->payload); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan8_scaled, i, msg->payload); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - i += put_int16_t_by_index(chan1_scaled, i, msg->payload); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan2_scaled, i, msg->payload); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan3_scaled, i, msg->payload); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan4_scaled, i, msg->payload); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan5_scaled, i, msg->payload); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan6_scaled, i, msg->payload); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan7_scaled, i, msg->payload); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan8_scaled, i, msg->payload); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, * @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_message_t msg; - mavlink_msg_rc_channels_scaled_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg.payload[0]; + + 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% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; + msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_rc_channels_scaled_t *p = &payload; + + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,10 +195,8 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, i */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan1_scaled); } /** @@ -147,10 +206,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan2_scaled); } /** @@ -160,10 +217,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan3_scaled); } /** @@ -173,10 +228,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan4_scaled); } /** @@ -186,10 +239,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan5_scaled); } /** @@ -199,10 +250,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan6_scaled); } /** @@ -212,10 +261,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan7_scaled); } /** @@ -225,10 +272,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan8_scaled); } /** @@ -238,7 +283,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavl */ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (uint8_t)(p->rssi); } /** @@ -249,13 +295,5 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_mess */ static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t* msg, mavlink_rc_channels_scaled_t* rc_channels_scaled) { - rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg); - rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg); - rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg); - rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg); - rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg); - rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg); - rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg); - rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg); - rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); + memcpy( rc_channels_scaled, msg->payload, sizeof(mavlink_rc_channels_scaled_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h index f390e4e370..4d48f09540 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -1,6 +1,8 @@ // MESSAGE REQUEST_DATA_STREAM PACKING #define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 +#define MAVLINK_MSG_66_LEN 6 typedef struct __mavlink_request_data_stream_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_request_data_stream_t } mavlink_request_data_stream_t; - - /** * @brief Pack a request_data_stream message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_request_data_stream_t */ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { - uint16_t i = 0; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type - i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { - uint16_t i = 0; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type - i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @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_message_t msg; - mavlink_msg_request_data_stream_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, req_stream_id, req_message_rate, start_stop); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; + msg.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_request_data_stream_t *p = &payload; + + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; + hdr.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -120,7 +174,8 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const ma */ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -130,7 +185,8 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const */ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->req_stream_id); } /** @@ -140,10 +196,8 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma */ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint16_t)(p->req_message_rate); } /** @@ -153,7 +207,8 @@ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(cons */ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->start_stop); } /** @@ -164,9 +219,5 @@ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavli */ static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream) { - request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg); - request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg); - request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); - request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); - request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); + memcpy( request_data_stream, msg->payload, sizeof(mavlink_request_data_stream_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h index e1f9cc6b12..aaee3bf13a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -1,6 +1,8 @@ // MESSAGE SAFETY_ALLOWED_AREA PACKING #define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 +#define MAVLINK_MSG_54_LEN 25 typedef struct __mavlink_safety_allowed_area_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_safety_allowed_area_t } mavlink_safety_allowed_area_t; - - /** * @brief Pack a safety_allowed_area message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_safety_allowed_area_t */ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, * @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_message_t msg; - mavlink_msg_safety_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; + msg.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_safety_allowed_area_t *p = &payload; + + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; + hdr.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -132,12 +190,8 @@ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_me */ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1x); } /** @@ -147,12 +201,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1y); } /** @@ -162,12 +212,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1z); } /** @@ -177,12 +223,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2x); } /** @@ -192,12 +234,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2y); } /** @@ -207,12 +245,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2z); } /** @@ -223,11 +257,5 @@ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_messag */ static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area) { - safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); - safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); - safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); - safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); - safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); - safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); - safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); + memcpy( safety_allowed_area, msg->payload, sizeof(mavlink_safety_allowed_area_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h index da571e8e72..4427dbedd7 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,6 +1,8 @@ // MESSAGE SAFETY_SET_ALLOWED_AREA PACKING #define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 +#define MAVLINK_MSG_53_LEN 27 typedef struct __mavlink_safety_set_allowed_area_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_safety_set_allowed_area_t } mavlink_safety_set_allowed_area_t; - - /** * @brief Pack a safety_set_allowed_area message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_safety_set_allowed_area_t */ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system * @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_message_t msg; - mavlink_msg_safety_set_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; + msg.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_safety_set_allowed_area_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; + hdr.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +195,8 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -144,7 +206,8 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(cons */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -154,7 +217,8 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(c */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -164,12 +228,8 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlin */ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1x); } /** @@ -179,12 +239,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1y); } /** @@ -194,12 +250,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1z); } /** @@ -209,12 +261,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2x); } /** @@ -224,12 +272,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2y); } /** @@ -239,12 +283,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2z); } /** @@ -255,13 +295,5 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_me */ static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area) { - safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); - safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); - safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); - safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); - safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); - safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); - safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); - safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); - safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); + memcpy( safety_set_allowed_area, msg->payload, sizeof(mavlink_safety_set_allowed_area_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 77637286ff..44fe8bdbdd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -1,6 +1,8 @@ // MESSAGE SCALED_IMU PACKING #define MAVLINK_MSG_ID_SCALED_IMU 26 +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 26 +#define MAVLINK_MSG_26_LEN 26 typedef struct __mavlink_scaled_imu_t { @@ -17,8 +19,6 @@ typedef struct __mavlink_scaled_imu_t } mavlink_scaled_imu_t; - - /** * @brief Pack a scaled_imu message * @param system_id ID of this system @@ -39,21 +39,21 @@ typedef struct __mavlink_scaled_imu_t */ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (millirad /sec) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (millirad /sec) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (millirad /sec) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (milli tesla) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (milli tesla) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (milli tesla) - - return mavlink_finalize_message(msg, system_id, component_id, i); + 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); } /** @@ -76,21 +76,21 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (millirad /sec) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (millirad /sec) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (millirad /sec) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (milli tesla) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (milli tesla) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (milli tesla) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + 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); } /** @@ -122,12 +122,75 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t * @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_message_t msg; - mavlink_msg_scaled_imu_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; + msg.msgid = MAVLINK_MSG_ID_SCALED_IMU; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_scaled_imu_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; + hdr.msgid = MAVLINK_MSG_ID_SCALED_IMU; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -140,16 +203,8 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_scaled_imu_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -159,10 +214,8 @@ static inline uint64_t mavlink_msg_scaled_imu_get_usec(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xacc); } /** @@ -172,10 +225,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->yacc); } /** @@ -185,10 +236,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zacc); } /** @@ -198,10 +247,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xgyro); } /** @@ -211,10 +258,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->ygyro); } /** @@ -224,10 +269,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zgyro); } /** @@ -237,10 +280,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xmag); } /** @@ -250,10 +291,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->ymag); } /** @@ -263,10 +302,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zmag); } /** @@ -277,14 +314,5 @@ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* m */ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, mavlink_scaled_imu_t* scaled_imu) { - scaled_imu->usec = mavlink_msg_scaled_imu_get_usec(msg); - scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg); - scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg); - scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg); - scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg); - scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg); - scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg); - scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); - scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); - scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); + memcpy( scaled_imu, msg->payload, sizeof(mavlink_scaled_imu_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 08a9bedd37..bf653ad5a3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -1,6 +1,8 @@ // MESSAGE SCALED_PRESSURE PACKING #define MAVLINK_MSG_ID_SCALED_PRESSURE 38 +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 18 +#define MAVLINK_MSG_38_LEN 18 typedef struct __mavlink_scaled_pressure_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_scaled_pressure_t } mavlink_scaled_pressure_t; - - /** * @brief Pack a scaled_pressure message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_scaled_pressure_t */ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { - uint16_t i = 0; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(press_abs, i, msg->payload); // Absolute pressure (hectopascal) - i += put_float_by_index(press_diff, i, msg->payload); // Differential pressure 1 (hectopascal) - i += put_int16_t_by_index(temperature, i, msg->payload); // Temperature measurement (0.01 degrees celsius) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { - uint16_t i = 0; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(press_abs, i, msg->payload); // Absolute pressure (hectopascal) - i += put_float_by_index(press_diff, i, msg->payload); // Differential pressure 1 (hectopascal) - i += put_int16_t_by_index(temperature, i, msg->payload); // Temperature measurement (0.01 degrees celsius) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_scaled_pressure_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, press_abs, press_diff, temperature); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; + msg.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_scaled_pressure_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,16 +155,8 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_scaled_pressure_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -123,12 +166,8 @@ static inline uint64_t mavlink_msg_scaled_pressure_get_usec(const mavlink_messag */ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (float)(p->press_abs); } /** @@ -138,12 +177,8 @@ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_mess */ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (float)(p->press_diff); } /** @@ -153,10 +188,8 @@ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_mes */ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (int16_t)(p->temperature); } /** @@ -167,8 +200,5 @@ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_ */ static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* msg, mavlink_scaled_pressure_t* scaled_pressure) { - scaled_pressure->usec = mavlink_msg_scaled_pressure_get_usec(msg); - scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); - scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); - scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); + memcpy( scaled_pressure, msg->payload, sizeof(mavlink_scaled_pressure_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h index 751afcb80a..f12eb29a0d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -1,6 +1,8 @@ // MESSAGE SERVO_OUTPUT_RAW PACKING #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 16 +#define MAVLINK_MSG_37_LEN 16 typedef struct __mavlink_servo_output_raw_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_servo_output_raw_t } mavlink_servo_output_raw_t; - - /** * @brief Pack a servo_output_raw message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_servo_output_raw_t */ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - uint16_t i = 0; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - uint16_t i = 0; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui * @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_message_t msg; - mavlink_msg_servo_output_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; + msg.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_servo_output_raw_t *p = &payload; + + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,10 +187,8 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo1_raw); } /** @@ -141,10 +198,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo2_raw); } /** @@ -154,10 +209,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo3_raw); } /** @@ -167,10 +220,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo4_raw); } /** @@ -180,10 +231,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo5_raw); } /** @@ -193,10 +242,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo6_raw); } /** @@ -206,10 +253,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo7_raw); } /** @@ -219,10 +264,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo8_raw); } /** @@ -233,12 +276,5 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink */ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw) { - servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); - servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); - servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); - servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); - servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); - servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); - servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); - servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); + memcpy( servo_output_raw, msg->payload, sizeof(mavlink_servo_output_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 775db90e34..3e32eee15f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -1,6 +1,8 @@ // MESSAGE SET_ALTITUDE PACKING #define MAVLINK_MSG_ID_SET_ALTITUDE 65 +#define MAVLINK_MSG_ID_SET_ALTITUDE_LEN 5 +#define MAVLINK_MSG_65_LEN 5 typedef struct __mavlink_set_altitude_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_altitude_t } mavlink_set_altitude_t; - - /** * @brief Pack a set_altitude message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_altitude_t */ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint32_t mode) { - uint16_t i = 0; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); // The new altitude in meters + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_set_altitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint32_t mode) { - uint16_t i = 0; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); // The new altitude in meters + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_set_altitude_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, mode); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_set_altitude_t *p = &payload; + + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,12 +150,8 @@ static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_ */ static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; + return (uint32_t)(p->mode); } /** @@ -118,6 +162,5 @@ static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t */ static inline void mavlink_msg_set_altitude_decode(const mavlink_message_t* msg, mavlink_set_altitude_t* set_altitude) { - set_altitude->target = mavlink_msg_set_altitude_get_target(msg); - set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); + memcpy( set_altitude, msg->payload, sizeof(mavlink_set_altitude_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index efeea6509f..a74108ee8e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -1,6 +1,8 @@ // MESSAGE SET_MODE PACKING #define MAVLINK_MSG_ID_SET_MODE 11 +#define MAVLINK_MSG_ID_SET_MODE_LEN 2 +#define MAVLINK_MSG_11_LEN 2 typedef struct __mavlink_set_mode_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_mode_t } mavlink_set_mode_t; - - /** * @brief Pack a set_mode message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_mode_t */ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t mode) { - uint16_t i = 0; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(mode, i, msg->payload); // The new mode + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t mode) { - uint16_t i = 0; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(mode, i, msg->payload); // The new mode + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co * @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_message_t msg; - mavlink_msg_set_mode_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, mode); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_MODE_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_MODE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_set_mode_t *p = &payload; + + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_MODE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_MODE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,7 +150,8 @@ static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -113,6 +162,5 @@ static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg */ static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mavlink_set_mode_t* set_mode) { - set_mode->target = mavlink_msg_set_mode_get_target(msg); - set_mode->mode = mavlink_msg_set_mode_get_mode(msg); + memcpy( set_mode, msg->payload, sizeof(mavlink_set_mode_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h index fe514b2ea5..649fbee575 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -1,6 +1,8 @@ // MESSAGE SET_NAV_MODE PACKING #define MAVLINK_MSG_ID_SET_NAV_MODE 12 +#define MAVLINK_MSG_ID_SET_NAV_MODE_LEN 2 +#define MAVLINK_MSG_12_LEN 2 typedef struct __mavlink_set_nav_mode_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_nav_mode_t } mavlink_set_nav_mode_t; - - /** * @brief Pack a set_nav_mode message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_nav_mode_t */ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t nav_mode) { - uint16_t i = 0; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // The new navigation mode + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_set_nav_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t nav_mode) { - uint16_t i = 0; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // The new navigation mode + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_set_nav_mode_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, nav_mode); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_set_nav_mode_t *p = &payload; + + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,7 +150,8 @@ static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_ */ static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -113,6 +162,5 @@ static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_messag */ static inline void mavlink_msg_set_nav_mode_decode(const mavlink_message_t* msg, mavlink_set_nav_mode_t* set_nav_mode) { - set_nav_mode->target = mavlink_msg_set_nav_mode_get_target(msg); - set_nav_mode->nav_mode = mavlink_msg_set_nav_mode_get_nav_mode(msg); + memcpy( set_nav_mode, msg->payload, sizeof(mavlink_set_nav_mode_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index c4c6b5abc9..6ca16a5347 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -1,6 +1,8 @@ // MESSAGE STATE_CORRECTION PACKING #define MAVLINK_MSG_ID_STATE_CORRECTION 64 +#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 +#define MAVLINK_MSG_64_LEN 36 typedef struct __mavlink_state_correction_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_state_correction_t } mavlink_state_correction_t; - - /** * @brief Pack a state_correction message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_state_correction_t */ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { - uint16_t i = 0; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - i += put_float_by_index(xErr, i, msg->payload); // x position error - i += put_float_by_index(yErr, i, msg->payload); // y position error - i += put_float_by_index(zErr, i, msg->payload); // z position error - i += put_float_by_index(rollErr, i, msg->payload); // roll error (radians) - i += put_float_by_index(pitchErr, i, msg->payload); // pitch error (radians) - i += put_float_by_index(yawErr, i, msg->payload); // yaw error (radians) - i += put_float_by_index(vxErr, i, msg->payload); // x velocity - i += put_float_by_index(vyErr, i, msg->payload); // y velocity - i += put_float_by_index(vzErr, i, msg->payload); // z velocity + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { - uint16_t i = 0; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - i += put_float_by_index(xErr, i, msg->payload); // x position error - i += put_float_by_index(yErr, i, msg->payload); // y position error - i += put_float_by_index(zErr, i, msg->payload); // z position error - i += put_float_by_index(rollErr, i, msg->payload); // roll error (radians) - i += put_float_by_index(pitchErr, i, msg->payload); // pitch error (radians) - i += put_float_by_index(yawErr, i, msg->payload); // yaw error (radians) - i += put_float_by_index(vxErr, i, msg->payload); // x velocity - i += put_float_by_index(vyErr, i, msg->payload); // y velocity - i += put_float_by_index(vzErr, i, msg->payload); // z velocity + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui * @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_message_t msg; - mavlink_msg_state_correction_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; + msg.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_state_correction_t *p = &payload; + + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,12 +195,8 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->xErr); } /** @@ -149,12 +206,8 @@ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->yErr); } /** @@ -164,12 +217,8 @@ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->zErr); } /** @@ -179,12 +228,8 @@ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->rollErr); } /** @@ -194,12 +239,8 @@ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_messa */ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->pitchErr); } /** @@ -209,12 +250,8 @@ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_mess */ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->yawErr); } /** @@ -224,12 +261,8 @@ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_messag */ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vxErr); } /** @@ -239,12 +272,8 @@ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vyErr); } /** @@ -254,12 +283,8 @@ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vzErr); } /** @@ -270,13 +295,5 @@ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message */ static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* msg, mavlink_state_correction_t* state_correction) { - state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg); - state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg); - state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg); - state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg); - state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg); - state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg); - state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg); - state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); - state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); + memcpy( state_correction, msg->payload, sizeof(mavlink_state_correction_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index 681b659c4d..485bbf1ccc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -1,17 +1,17 @@ // MESSAGE STATUSTEXT PACKING #define MAVLINK_MSG_ID_STATUSTEXT 254 +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_254_LEN 51 typedef struct __mavlink_statustext_t { uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault - int8_t text[50]; ///< Status text message, without null termination character + char text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; - #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 - /** * @brief Pack a statustext message * @param system_id ID of this system @@ -22,15 +22,15 @@ typedef struct __mavlink_statustext_t * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const int8_t* text) +static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const char* text) { - uint16_t i = 0; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } /** @@ -43,15 +43,15 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const int8_t* text) +static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const char* text) { - uint16_t i = 0; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param text Status text message, without null termination character */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text) +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) { mavlink_message_t msg; - mavlink_msg_statustext_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, severity, text); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; + msg.msgid = MAVLINK_MSG_ID_STATUSTEXT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_statustext_t *p = &payload; + + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; + hdr.msgid = MAVLINK_MSG_ID_STATUSTEXT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -93,7 +140,8 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; + return (uint8_t)(p->severity); } /** @@ -101,11 +149,12 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_ * * @return Status text message, without null termination character */ -static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char* text) { + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t), sizeof(int8_t)*50); - return sizeof(int8_t)*50; + memcpy(text, p->text, sizeof(p->text)); + return sizeof(p->text); } /** @@ -116,6 +165,5 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* */ static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) { - statustext->severity = mavlink_msg_statustext_get_severity(msg); - mavlink_msg_statustext_get_text(msg, statustext->text); + memcpy( statustext, msg->payload, sizeof(mavlink_statustext_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index ef83d84481..a340cfaa8c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -1,6 +1,8 @@ // MESSAGE SYS_STATUS PACKING #define MAVLINK_MSG_ID_SYS_STATUS 34 +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 11 +#define MAVLINK_MSG_34_LEN 11 typedef struct __mavlink_sys_status_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_sys_status_t } mavlink_sys_status_t; - - /** * @brief Pack a sys_status message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_sys_status_t */ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - uint16_t i = 0; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM - i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) - i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - uint16_t i = 0; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM - i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) - i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_message_t msg; - mavlink_msg_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg.payload[0]; + + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_SYS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) +{ + mavlink_header_t hdr; + mavlink_sys_status_t payload; + uint16_t checksum; + mavlink_sys_status_t *p = &payload; + + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t m */ static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -132,7 +190,8 @@ static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -142,7 +201,8 @@ static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_ */ static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->status); } /** @@ -152,10 +212,8 @@ static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->load); } /** @@ -165,10 +223,8 @@ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->vbat); } /** @@ -178,10 +234,8 @@ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->battery_remaining); } /** @@ -191,10 +245,8 @@ static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlin */ static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->packet_drop); } /** @@ -205,11 +257,5 @@ static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_mess */ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) { - sys_status->mode = mavlink_msg_sys_status_get_mode(msg); - sys_status->nav_mode = mavlink_msg_sys_status_get_nav_mode(msg); - sys_status->status = mavlink_msg_sys_status_get_status(msg); - sys_status->load = mavlink_msg_sys_status_get_load(msg); - sys_status->vbat = mavlink_msg_sys_status_get_vbat(msg); - sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); - sys_status->packet_drop = mavlink_msg_sys_status_get_packet_drop(msg); + memcpy( sys_status, msg->payload, sizeof(mavlink_sys_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index d293f025bf..2213495fc0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -1,6 +1,8 @@ // MESSAGE SYSTEM_TIME PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME 2 +#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 8 +#define MAVLINK_MSG_2_LEN 8 typedef struct __mavlink_system_time_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_system_time_t } mavlink_system_time_t; - - /** * @brief Pack a system_time message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_system_time_t */ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec) { - uint16_t i = 0; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - i += put_uint64_t_by_index(time_usec, i, msg->payload); // Timestamp of the master clock in microseconds since UNIX epoch. + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_usec) { - uint16_t i = 0; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - i += put_uint64_t_by_index(time_usec, i, msg->payload); // Timestamp of the master clock in microseconds since UNIX epoch. + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { mavlink_message_t msg; - mavlink_msg_system_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_usec); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg.payload[0]; + + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; + msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) +{ + mavlink_header_t hdr; + mavlink_system_time_t payload; + uint16_t checksum; + mavlink_system_time_t *p = &payload; + + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,16 +131,8 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; + return (uint64_t)(p->time_usec); } /** @@ -106,5 +143,5 @@ static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_messa */ static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) { - system_time->time_usec = mavlink_msg_system_time_get_time_usec(msg); + memcpy( system_time, msg->payload, sizeof(mavlink_system_time_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h index 75c25f1ced..04d124f2b5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -1,6 +1,8 @@ // MESSAGE SYSTEM_TIME_UTC PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 +#define MAVLINK_MSG_4_LEN 8 typedef struct __mavlink_system_time_utc_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_system_time_utc_t } mavlink_system_time_utc_t; - - /** * @brief Pack a system_time_utc message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_system_time_utc_t */ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t utc_date, uint32_t utc_time) { - uint16_t i = 0; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t utc_date, uint32_t utc_time) { - uint16_t i = 0; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_system_time_utc_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, utc_date, utc_time); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg.payload[0]; + + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; + msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_system_time_utc_t *p = &payload; + + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,12 +139,8 @@ static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint */ static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; + return (uint32_t)(p->utc_date); } /** @@ -107,12 +150,8 @@ static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_me */ static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; + return (uint32_t)(p->utc_time); } /** @@ -123,6 +162,5 @@ static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_me */ static inline void mavlink_msg_system_time_utc_decode(const mavlink_message_t* msg, mavlink_system_time_utc_t* system_time_utc) { - system_time_utc->utc_date = mavlink_msg_system_time_utc_get_utc_date(msg); - system_time_utc->utc_time = mavlink_msg_system_time_utc_get_utc_time(msg); + memcpy( system_time_utc, msg->payload, sizeof(mavlink_system_time_utc_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index 9f55bdfa70..ee92a1e34c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -1,6 +1,8 @@ // MESSAGE VFR_HUD PACKING #define MAVLINK_MSG_ID_VFR_HUD 74 +#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 +#define MAVLINK_MSG_74_LEN 20 typedef struct __mavlink_vfr_hud_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_vfr_hud_t } mavlink_vfr_hud_t; - - /** * @brief Pack a vfr_hud message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_vfr_hud_t */ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { - uint16_t i = 0; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, in compass units (0..360, 0=north) - i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 - i += put_float_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters - i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { - uint16_t i = 0; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, in compass units (0..360, 0=north) - i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 - i += put_float_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters - i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com * @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_message_t msg; - mavlink_msg_vfr_hud_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, airspeed, groundspeed, heading, throttle, alt, climb); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VFR_HUD_LEN; + msg.msgid = MAVLINK_MSG_ID_VFR_HUD; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_vfr_hud_t *p = &payload; + + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VFR_HUD_LEN; + hdr.msgid = MAVLINK_MSG_ID_VFR_HUD; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe */ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->airspeed); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* ms */ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->groundspeed); } /** @@ -146,10 +193,8 @@ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* */ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (int16_t)(p->heading); } /** @@ -159,10 +204,8 @@ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* m */ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (uint16_t)r.s; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (uint16_t)(p->throttle); } /** @@ -172,12 +215,8 @@ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* */ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -187,12 +226,8 @@ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->climb); } /** @@ -203,10 +238,5 @@ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) */ static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud) { - vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); - vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); - vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); - vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); - vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); - vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); + memcpy( vfr_hud, msg->payload, sizeof(mavlink_vfr_hud_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index ab1c5f836a..07f7a08bd1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT PACKING #define MAVLINK_MSG_ID_WAYPOINT 39 +#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 +#define MAVLINK_MSG_39_LEN 36 typedef struct __mavlink_waypoint_t { @@ -21,8 +23,6 @@ typedef struct __mavlink_waypoint_t } mavlink_waypoint_t; - - /** * @brief Pack a waypoint message * @param system_id ID of this system @@ -47,25 +47,25 @@ typedef struct __mavlink_waypoint_t */ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { - uint16_t i = 0; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - i += put_uint8_t_by_index(frame, i, msg->payload); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - i += put_uint8_t_by_index(command, i, msg->payload); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - i += put_uint8_t_by_index(current, i, msg->payload); // false:0, true:1 - i += put_uint8_t_by_index(autocontinue, i, msg->payload); // autocontinue to next wp - i += put_float_by_index(param1, i, msg->payload); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - i += put_float_by_index(param3, i, msg->payload); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude - i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_LEN); } /** @@ -92,25 +92,25 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_waypoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { - uint16_t i = 0; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - i += put_uint8_t_by_index(frame, i, msg->payload); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - i += put_uint8_t_by_index(command, i, msg->payload); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - i += put_uint8_t_by_index(current, i, msg->payload); // false:0, true:1 - i += put_uint8_t_by_index(autocontinue, i, msg->payload); // autocontinue to next wp - i += put_float_by_index(param1, i, msg->payload); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - i += put_float_by_index(param3, i, msg->payload); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude - i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_LEN); } /** @@ -146,12 +146,83 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co * @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_message_t msg; - mavlink_msg_waypoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -164,7 +235,8 @@ static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -174,7 +246,8 @@ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_messa */ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -184,10 +257,8 @@ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_me */ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -197,7 +268,8 @@ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -207,7 +279,8 @@ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->command); } /** @@ -217,7 +290,8 @@ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->current); } /** @@ -227,7 +301,8 @@ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->autocontinue); } /** @@ -237,12 +312,8 @@ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_messag */ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param1); } /** @@ -252,12 +323,8 @@ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param2); } /** @@ -267,12 +334,8 @@ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param3); } /** @@ -282,12 +345,8 @@ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param4); } /** @@ -297,12 +356,8 @@ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -312,12 +367,8 @@ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -327,12 +378,8 @@ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -343,18 +390,5 @@ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) */ static inline void mavlink_msg_waypoint_decode(const mavlink_message_t* msg, mavlink_waypoint_t* waypoint) { - waypoint->target_system = mavlink_msg_waypoint_get_target_system(msg); - waypoint->target_component = mavlink_msg_waypoint_get_target_component(msg); - waypoint->seq = mavlink_msg_waypoint_get_seq(msg); - waypoint->frame = mavlink_msg_waypoint_get_frame(msg); - waypoint->command = mavlink_msg_waypoint_get_command(msg); - waypoint->current = mavlink_msg_waypoint_get_current(msg); - waypoint->autocontinue = mavlink_msg_waypoint_get_autocontinue(msg); - waypoint->param1 = mavlink_msg_waypoint_get_param1(msg); - waypoint->param2 = mavlink_msg_waypoint_get_param2(msg); - waypoint->param3 = mavlink_msg_waypoint_get_param3(msg); - waypoint->param4 = mavlink_msg_waypoint_get_param4(msg); - waypoint->x = mavlink_msg_waypoint_get_x(msg); - waypoint->y = mavlink_msg_waypoint_get_y(msg); - waypoint->z = mavlink_msg_waypoint_get_z(msg); + memcpy( waypoint, msg->payload, sizeof(mavlink_waypoint_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index d994eaf49a..8bdd5ae15a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_ACK PACKING #define MAVLINK_MSG_ID_WAYPOINT_ACK 47 +#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 +#define MAVLINK_MSG_47_LEN 3 typedef struct __mavlink_waypoint_ack_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_ack_t } mavlink_waypoint_ack_t; - - /** * @brief Pack a waypoint_ack message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_ack_t */ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t type) { - uint16_t i = 0; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(type, i, msg->payload); // 0: OK, 1: Error + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_waypoint_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t type) { - uint16_t i = 0; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(type, i, msg->payload); // 0: OK, 1: Error + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_waypoint_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, type); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_ack_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,7 +169,8 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlin */ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -129,7 +181,5 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* */ static inline void mavlink_msg_waypoint_ack_decode(const mavlink_message_t* msg, mavlink_waypoint_ack_t* waypoint_ack) { - waypoint_ack->target_system = mavlink_msg_waypoint_ack_get_target_system(msg); - waypoint_ack->target_component = mavlink_msg_waypoint_ack_get_target_component(msg); - waypoint_ack->type = mavlink_msg_waypoint_ack_get_type(msg); + memcpy( waypoint_ack, msg->payload, sizeof(mavlink_waypoint_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h index 5020fbada8..efccf36008 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_CLEAR_ALL PACKING #define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 +#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_45_LEN 2 typedef struct __mavlink_waypoint_clear_all_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_waypoint_clear_all_t } mavlink_waypoint_clear_all_t; - - /** * @brief Pack a waypoint_clear_all message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_waypoint_clear_all_t */ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_waypoint_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t 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_message_t msg; - mavlink_msg_waypoint_clear_all_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_clear_all_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +150,8 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mav */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +162,5 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const */ static inline void mavlink_msg_waypoint_clear_all_decode(const mavlink_message_t* msg, mavlink_waypoint_clear_all_t* waypoint_clear_all) { - waypoint_clear_all->target_system = mavlink_msg_waypoint_clear_all_get_target_system(msg); - waypoint_clear_all->target_component = mavlink_msg_waypoint_clear_all_get_target_component(msg); + memcpy( waypoint_clear_all, msg->payload, sizeof(mavlink_waypoint_clear_all_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 9ceacb0806..a446197215 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_COUNT PACKING #define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 +#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 +#define MAVLINK_MSG_44_LEN 4 typedef struct __mavlink_waypoint_count_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_count_t } mavlink_waypoint_count_t; - - /** * @brief Pack a waypoint_count message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_count_t */ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count) { - uint16_t i = 0; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(count, i, msg->payload); // Number of Waypoints in the Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_waypoint_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count) { - uint16_t i = 0; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(count, i, msg->payload); // Number of Waypoints in the Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint * @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_message_t msg; - mavlink_msg_waypoint_count_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, count); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_count_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink */ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavl */ static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint16_t)(p->count); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_messag */ static inline void mavlink_msg_waypoint_count_decode(const mavlink_message_t* msg, mavlink_waypoint_count_t* waypoint_count) { - waypoint_count->target_system = mavlink_msg_waypoint_count_get_target_system(msg); - waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); - waypoint_count->count = mavlink_msg_waypoint_count_get_count(msg); + memcpy( waypoint_count, msg->payload, sizeof(mavlink_waypoint_count_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 17f66fc298..59b2f6b3a7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_CURRENT PACKING #define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 +#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 +#define MAVLINK_MSG_42_LEN 2 typedef struct __mavlink_waypoint_current_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_waypoint_current_t } mavlink_waypoint_current_t; - - /** * @brief Pack a waypoint_current message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_waypoint_current_t */ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui * @param seq Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { mavlink_message_t msg; - mavlink_msg_waypoint_current_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg.payload[0]; + + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) +{ + mavlink_header_t hdr; + mavlink_waypoint_current_t payload; + uint16_t checksum; + mavlink_waypoint_current_t *p = &payload; + + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,10 +131,8 @@ static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -100,5 +143,5 @@ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_messag */ static inline void mavlink_msg_waypoint_current_decode(const mavlink_message_t* msg, mavlink_waypoint_current_t* waypoint_current) { - waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); + memcpy( waypoint_current, msg->payload, sizeof(mavlink_waypoint_current_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index ffcea52fc9..17404e658b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REACHED PACKING #define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 +#define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 +#define MAVLINK_MSG_46_LEN 2 typedef struct __mavlink_waypoint_reached_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_waypoint_reached_t } mavlink_waypoint_reached_t; - - /** * @brief Pack a waypoint_reached message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_waypoint_reached_t */ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui * @param seq Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { mavlink_message_t msg; - mavlink_msg_waypoint_reached_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg.payload[0]; + + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) +{ + mavlink_header_t hdr; + mavlink_waypoint_reached_t payload; + uint16_t checksum; + mavlink_waypoint_reached_t *p = &payload; + + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,10 +131,8 @@ static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -100,5 +143,5 @@ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_messag */ static inline void mavlink_msg_waypoint_reached_decode(const mavlink_message_t* msg, mavlink_waypoint_reached_t* waypoint_reached) { - waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); + memcpy( waypoint_reached, msg->payload, sizeof(mavlink_waypoint_reached_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index 234b55c11d..c33edce8e2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REQUEST PACKING #define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 +#define MAVLINK_MSG_40_LEN 4 typedef struct __mavlink_waypoint_request_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_request_t } mavlink_waypoint_request_t; - - /** * @brief Pack a waypoint_request message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_request_t */ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_waypoint_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui * @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_message_t msg; - mavlink_msg_waypoint_request_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_request_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavli */ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const ma */ static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_messag */ static inline void mavlink_msg_waypoint_request_decode(const mavlink_message_t* msg, mavlink_waypoint_request_t* waypoint_request) { - waypoint_request->target_system = mavlink_msg_waypoint_request_get_target_system(msg); - waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); - waypoint_request->seq = mavlink_msg_waypoint_request_get_seq(msg); + memcpy( waypoint_request, msg->payload, sizeof(mavlink_waypoint_request_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h index d4f7cf03d5..209b6140eb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_43_LEN 2 typedef struct __mavlink_waypoint_request_list_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_waypoint_request_list_t } mavlink_waypoint_request_list_t; - - /** * @brief Pack a waypoint_request_list message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_waypoint_request_list_t */ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_waypoint_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i * @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_message_t msg; - mavlink_msg_waypoint_request_list_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_request_list_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +150,8 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +162,5 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(con */ static inline void mavlink_msg_waypoint_request_list_decode(const mavlink_message_t* msg, mavlink_waypoint_request_list_t* waypoint_request_list) { - waypoint_request_list->target_system = mavlink_msg_waypoint_request_list_get_target_system(msg); - waypoint_request_list->target_component = mavlink_msg_waypoint_request_list_get_target_component(msg); + memcpy( waypoint_request_list, msg->payload, sizeof(mavlink_waypoint_request_list_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h index 6570fe4f82..c35fe21075 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_SET_CURRENT PACKING #define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 +#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 +#define MAVLINK_MSG_41_LEN 4 typedef struct __mavlink_waypoint_set_current_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_set_current_t } mavlink_waypoint_set_current_t; - - /** * @brief Pack a waypoint_set_current message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_set_current_t */ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_waypoint_set_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_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_message_t msg; - mavlink_msg_waypoint_set_current_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_waypoint_set_current_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const m */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(cons */ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_me */ static inline void mavlink_msg_waypoint_set_current_decode(const mavlink_message_t* msg, mavlink_waypoint_set_current_t* waypoint_set_current) { - waypoint_set_current->target_system = mavlink_msg_waypoint_set_current_get_target_system(msg); - waypoint_set_current->target_component = mavlink_msg_waypoint_set_current_get_target_component(msg); - waypoint_set_current->seq = mavlink_msg_waypoint_set_current_get_seq(msg); + memcpy( waypoint_set_current, msg->payload, sizeof(mavlink_waypoint_set_current_t)); } diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index 67eed17c8e..199fc56f49 100644 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -3,172 +3,7 @@ #include "inttypes.h" -enum MAV_CLASS -{ - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch - MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu - MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NONE = 8, ///< No valid autopilot - MAV_CLASS_NB ///< Number of autopilot classes -}; - -enum MAV_ACTION -{ - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions -}; - -enum MAV_MODE -{ - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back -}; - -enum MAV_STATE -{ - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_HILSIM, - MAV_STATE_POWEROFF -}; - -enum MAV_NAV -{ - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT -}; - -enum MAV_TYPE -{ - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 -}; - -enum MAV_AUTOPILOT_TYPE -{ - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 -}; - -enum MAV_COMPONENT -{ - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 -}; - -enum MAV_FRAME -{ - MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - MAV_FRAME_LOCAL_ENU = 4 -}; - -enum MAVLINK_DATA_STREAM_TYPE -{ - MAVLINK_DATA_STREAM_IMG_JPEG, - MAVLINK_DATA_STREAM_IMG_BMP, - MAVLINK_DATA_STREAM_IMG_RAW8U, - MAVLINK_DATA_STREAM_IMG_RAW32U, - MAVLINK_DATA_STREAM_IMG_PGM, - MAVLINK_DATA_STREAM_IMG_PNG - -}; - -#define MAVLINK_STX 0x55 ///< Packet start sign +#define MAVLINK_STX 0xD5 ///< Packet start sign #define MAVLINK_STX_LEN 1 ///< Length of start sign #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length @@ -191,36 +26,37 @@ typedef struct __mavlink_system { } mavlink_system_t; typedef struct __mavlink_message { + uint8_t ck_a; ///< Checksum high byte + uint8_t ck_b; ///< Checksum low byte + uint8_t STX; ///< start of packet marker uint8_t len; ///< Length of payload uint8_t seq; ///< Sequence of packet uint8_t sysid; ///< ID of message sender system/aircraft uint8_t compid; ///< ID of the message sender component uint8_t msgid; ///< ID of message in payload uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header { uint8_t ck_a; ///< Checksum high byte uint8_t ck_b; ///< Checksum low byte -} mavlink_message_t; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload +} mavlink_header_t; typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, - MAVLINK_COMM_3 + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 } 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, diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 45a5e9566a..7cfe0d9d02 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "minimal.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 0e5c4db5ca..fea5381e9c 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_0_LEN 3 typedef struct __mavlink_heartbeat_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_heartbeat_t } mavlink_heartbeat_t; - - /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -24,14 +24,14 @@ typedef struct __mavlink_heartbeat_t */ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message(msg, system_id, component_id, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -46,14 +46,14 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -77,12 +77,61 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) +{ + mavlink_header_t hdr; + mavlink_heartbeat_t payload; + uint16_t checksum; + mavlink_heartbeat_t *p = &payload; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -95,7 +144,8 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -105,7 +155,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->autopilot); } /** @@ -115,7 +166,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->mavlink_version); } /** @@ -126,7 +178,5 @@ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_me */ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) { - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); + memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index 61cd3fe22b..47a266ccb9 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -32,13 +32,6 @@ extern "C" { // MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index 16a37599e1..963c3697ab 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "pixhawk.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index 1a8b806f8a..25ed694100 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE_CONTROL PACKING #define MAVLINK_MSG_ID_ATTITUDE_CONTROL 85 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21 +#define MAVLINK_MSG_85_LEN 21 typedef struct __mavlink_attitude_control_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_attitude_control_t } mavlink_attitude_control_t; - - /** * @brief Pack a attitude_control message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_attitude_control_t */ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui * @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_message_t msg; - mavlink_msg_attitude_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_attitude_control_t *p = &payload; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +195,8 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -144,12 +206,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_mess */ static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -159,12 +217,8 @@ static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_ */ static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -174,12 +228,8 @@ static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message */ static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -189,12 +239,8 @@ static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t */ static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->thrust); } /** @@ -204,7 +250,8 @@ static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_messag */ static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->roll_manual); } /** @@ -214,7 +261,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink */ static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->pitch_manual); } /** @@ -224,7 +272,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlin */ static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->yaw_manual); } /** @@ -234,7 +283,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_ */ static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->thrust_manual); } /** @@ -245,13 +295,5 @@ static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavli */ static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* msg, mavlink_attitude_control_t* attitude_control) { - attitude_control->target = mavlink_msg_attitude_control_get_target(msg); - attitude_control->roll = mavlink_msg_attitude_control_get_roll(msg); - attitude_control->pitch = mavlink_msg_attitude_control_get_pitch(msg); - attitude_control->yaw = mavlink_msg_attitude_control_get_yaw(msg); - attitude_control->thrust = mavlink_msg_attitude_control_get_thrust(msg); - attitude_control->roll_manual = mavlink_msg_attitude_control_get_roll_manual(msg); - attitude_control->pitch_manual = mavlink_msg_attitude_control_get_pitch_manual(msg); - attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg); - attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg); + memcpy( attitude_control, msg->payload, sizeof(mavlink_attitude_control_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index f9dbde8c86..9cb8641e82 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -1,6 +1,8 @@ // MESSAGE AUX_STATUS PACKING #define MAVLINK_MSG_ID_AUX_STATUS 142 +#define MAVLINK_MSG_ID_AUX_STATUS_LEN 12 +#define MAVLINK_MSG_142_LEN 12 typedef struct __mavlink_aux_status_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_aux_status_t } mavlink_aux_status_t; - - /** * @brief Pack a aux_status message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_aux_status_t */ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - uint16_t i = 0; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUX_STATUS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_aux_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - uint16_t i = 0; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUX_STATUS_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param uart_total_err_count Number of I2C errors since startup */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { mavlink_message_t msg; - mavlink_msg_aux_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, load, i2c0_err_count, i2c1_err_count, spi0_err_count, spi1_err_count, uart_total_err_count); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg.payload[0]; + + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_AUX_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) +{ + mavlink_header_t hdr; + mavlink_aux_status_t payload; + uint16_t checksum; + mavlink_aux_status_t *p = &payload; + + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_AUX_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,10 +171,8 @@ static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t */ static inline uint16_t mavlink_msg_aux_status_get_load(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->load); } /** @@ -129,10 +182,8 @@ static inline uint16_t mavlink_msg_aux_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->i2c0_err_count); } /** @@ -142,10 +193,8 @@ static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->i2c1_err_count); } /** @@ -155,10 +204,8 @@ static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->spi0_err_count); } /** @@ -168,10 +215,8 @@ static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->spi1_err_count); } /** @@ -181,10 +226,8 @@ static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->uart_total_err_count); } /** @@ -195,10 +238,5 @@ static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(const mav */ static inline void mavlink_msg_aux_status_decode(const mavlink_message_t* msg, mavlink_aux_status_t* aux_status) { - aux_status->load = mavlink_msg_aux_status_get_load(msg); - aux_status->i2c0_err_count = mavlink_msg_aux_status_get_i2c0_err_count(msg); - aux_status->i2c1_err_count = mavlink_msg_aux_status_get_i2c1_err_count(msg); - aux_status->spi0_err_count = mavlink_msg_aux_status_get_spi0_err_count(msg); - aux_status->spi1_err_count = mavlink_msg_aux_status_get_spi1_err_count(msg); - aux_status->uart_total_err_count = mavlink_msg_aux_status_get_uart_total_err_count(msg); + memcpy( aux_status, msg->payload, sizeof(mavlink_aux_status_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index a61e13bcde..96814fcabc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -1,6 +1,8 @@ // MESSAGE BRIEF_FEATURE PACKING #define MAVLINK_MSG_ID_BRIEF_FEATURE 172 +#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 +#define MAVLINK_MSG_172_LEN 53 typedef struct __mavlink_brief_feature_t { @@ -10,14 +12,12 @@ typedef struct __mavlink_brief_feature_t uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true uint16_t size; ///< Size in pixels uint16_t orientation; ///< Orientation - uint8_t descriptor[32]; ///< Descriptor + char descriptor[32]; ///< Descriptor float response; ///< Harris operator response at this location } mavlink_brief_feature_t; - #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 - /** * @brief Pack a brief_feature message * @param system_id ID of this system @@ -34,21 +34,21 @@ 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 char* descriptor, float response) { - uint16_t i = 0; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } /** @@ -67,21 +67,21 @@ 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 char* descriptor, float response) { - uint16_t i = 0; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } /** @@ -111,12 +111,71 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param response Harris operator response at this location */ #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) +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 char* descriptor, float response) { mavlink_message_t msg; - mavlink_msg_brief_feature_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, orientation_assignment, size, orientation, descriptor, response); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg.payload[0]; + + 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)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; + msg.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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 char* descriptor, float response) +{ + mavlink_header_t hdr; + mavlink_brief_feature_t payload; + uint16_t checksum; + mavlink_brief_feature_t *p = &payload; + + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -129,12 +188,8 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float */ static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -144,12 +199,8 @@ static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg */ static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -159,12 +210,8 @@ static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg */ static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -174,7 +221,8 @@ static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint8_t)(p->orientation_assignment); } /** @@ -184,10 +232,8 @@ static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const */ static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint16_t)(p->size); } /** @@ -197,10 +243,8 @@ static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_ */ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint16_t)(p->orientation); } /** @@ -208,11 +252,12 @@ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_m * * @return Descriptor */ -static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t* r_data) +static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, char* descriptor) { + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t), sizeof(uint8_t)*32); - return sizeof(uint8_t)*32; + memcpy(descriptor, p->descriptor, sizeof(p->descriptor)); + return sizeof(p->descriptor); } /** @@ -222,12 +267,8 @@ static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_me */ static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->response); } /** @@ -238,12 +279,5 @@ static inline float mavlink_msg_brief_feature_get_response(const mavlink_message */ static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature) { - brief_feature->x = mavlink_msg_brief_feature_get_x(msg); - brief_feature->y = mavlink_msg_brief_feature_get_y(msg); - brief_feature->z = mavlink_msg_brief_feature_get_z(msg); - brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); - brief_feature->size = mavlink_msg_brief_feature_get_size(msg); - brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); - mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); - brief_feature->response = mavlink_msg_brief_feature_get_response(msg); + memcpy( brief_feature, msg->payload, sizeof(mavlink_brief_feature_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h index e9e69f7c9f..22116a54c8 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -1,6 +1,8 @@ // MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING #define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 170 +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 8 +#define MAVLINK_MSG_170_LEN 8 typedef struct __mavlink_data_transmission_handshake_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_data_transmission_handshake_t } mavlink_data_transmission_handshake_t; - - /** * @brief Pack a data_transmission_handshake message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_data_transmission_handshake_t */ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - uint16_t i = 0; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst */ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - uint16_t i = 0; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy * @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_message_t msg; - mavlink_msg_data_transmission_handshake_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, size, packets, payload, jpg_quality); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg.payload[0]; + + 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] + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; + msg.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_data_transmission_handshake_t *p = &payload; + + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; + hdr.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -120,12 +174,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mav */ static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint32_t)(p->size); } /** @@ -135,7 +185,8 @@ static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const ma */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->packets); } /** @@ -145,7 +196,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->payload); } /** @@ -155,7 +207,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->jpg_quality); } /** @@ -166,9 +219,5 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(co */ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) { - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); - data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); - data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); - data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); - data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); + memcpy( data_transmission_handshake, msg->payload, sizeof(mavlink_data_transmission_handshake_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index c2c040ae65..cd052fcb49 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -1,6 +1,8 @@ // MESSAGE ENCAPSULATED_DATA PACKING #define MAVLINK_MSG_ID_ENCAPSULATED_DATA 171 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 +#define MAVLINK_MSG_171_LEN 255 typedef struct __mavlink_encapsulated_data_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_encapsulated_data_t uint8_t data[253]; ///< image data bytes } mavlink_encapsulated_data_t; - #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 - /** * @brief Pack a encapsulated_data message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_encapsulated_data_t */ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) { - uint16_t i = 0; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) { - uint16_t i = 0; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param data image data bytes */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) { mavlink_message_t msg; - mavlink_msg_encapsulated_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seqnr, data); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; + msg.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_encapsulated_data_t *p = &payload; + + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; + hdr.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -93,10 +140,8 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui */ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; + return (uint16_t)(p->seqnr); } /** @@ -104,11 +149,12 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_mes * * @return image data bytes */ -static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* r_data) +static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* data) { + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t), sizeof(uint8_t)*253); - return sizeof(uint8_t)*253; + memcpy(data, p->data, sizeof(p->data)); + return sizeof(p->data); } /** @@ -119,6 +165,5 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_mess */ static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) { - encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); - mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); + memcpy( encapsulated_data, msg->payload, sizeof(mavlink_encapsulated_data_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index b40e23df7b..49869a01ec 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_AVAILABLE PACKING #define MAVLINK_MSG_ID_IMAGE_AVAILABLE 103 +#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 +#define MAVLINK_MSG_103_LEN 92 typedef struct __mavlink_image_available_t { @@ -30,8 +32,6 @@ typedef struct __mavlink_image_available_t } mavlink_image_available_t; - - /** * @brief Pack a image_available message * @param system_id ID of this system @@ -65,34 +65,34 @@ typedef struct __mavlink_image_available_t */ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } /** @@ -128,34 +128,34 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } /** @@ -200,12 +200,101 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_image_available_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; + msg.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_image_available_t *p = &payload; + + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -218,16 +307,8 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->cam_id); } /** @@ -237,7 +318,8 @@ static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_mess */ static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint8_t)(p->cam_no); } /** @@ -247,16 +329,8 @@ static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_messa */ static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->timestamp); } /** @@ -266,16 +340,8 @@ static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_m */ static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->valid_until); } /** @@ -285,12 +351,8 @@ static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink */ static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->img_seq); } /** @@ -300,12 +362,8 @@ static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_mes */ static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->img_buf_index); } /** @@ -315,10 +373,8 @@ static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavli */ static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->width); } /** @@ -328,10 +384,8 @@ static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_messa */ static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->height); } /** @@ -341,10 +395,8 @@ static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_mess */ static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->depth); } /** @@ -354,7 +406,8 @@ static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_messa */ static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint8_t)(p->channels); } /** @@ -364,12 +417,8 @@ static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_mes */ static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->key); } /** @@ -379,12 +428,8 @@ static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message */ static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->exposure); } /** @@ -394,12 +439,8 @@ static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_me */ static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->gain); } /** @@ -409,12 +450,8 @@ static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t */ static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -424,12 +461,8 @@ static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t */ static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -439,12 +472,8 @@ static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_ */ static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -454,12 +483,8 @@ static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_local_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->local_z); } /** @@ -469,12 +494,8 @@ static inline float mavlink_msg_image_available_get_local_z(const mavlink_messag */ static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -484,12 +505,8 @@ static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -499,12 +516,8 @@ static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -514,12 +527,8 @@ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_x); } /** @@ -529,12 +538,8 @@ static inline float mavlink_msg_image_available_get_ground_x(const mavlink_messa */ static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_y); } /** @@ -544,12 +549,8 @@ static inline float mavlink_msg_image_available_get_ground_y(const mavlink_messa */ static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_z); } /** @@ -560,27 +561,5 @@ static inline float mavlink_msg_image_available_get_ground_z(const mavlink_messa */ static inline void mavlink_msg_image_available_decode(const mavlink_message_t* msg, mavlink_image_available_t* image_available) { - image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg); - image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg); - image_available->timestamp = mavlink_msg_image_available_get_timestamp(msg); - image_available->valid_until = mavlink_msg_image_available_get_valid_until(msg); - image_available->img_seq = mavlink_msg_image_available_get_img_seq(msg); - image_available->img_buf_index = mavlink_msg_image_available_get_img_buf_index(msg); - image_available->width = mavlink_msg_image_available_get_width(msg); - image_available->height = mavlink_msg_image_available_get_height(msg); - image_available->depth = mavlink_msg_image_available_get_depth(msg); - image_available->channels = mavlink_msg_image_available_get_channels(msg); - image_available->key = mavlink_msg_image_available_get_key(msg); - image_available->exposure = mavlink_msg_image_available_get_exposure(msg); - image_available->gain = mavlink_msg_image_available_get_gain(msg); - image_available->roll = mavlink_msg_image_available_get_roll(msg); - image_available->pitch = mavlink_msg_image_available_get_pitch(msg); - image_available->yaw = mavlink_msg_image_available_get_yaw(msg); - image_available->local_z = mavlink_msg_image_available_get_local_z(msg); - image_available->lat = mavlink_msg_image_available_get_lat(msg); - image_available->lon = mavlink_msg_image_available_get_lon(msg); - image_available->alt = mavlink_msg_image_available_get_alt(msg); - image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg); - image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg); - image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg); + memcpy( image_available, msg->payload, sizeof(mavlink_image_available_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h index 90aa9dcf4e..0e81a6c6d2 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_TRIGGER_CONTROL PACKING #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 102 +#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 +#define MAVLINK_MSG_102_LEN 1 typedef struct __mavlink_image_trigger_control_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_image_trigger_control_t } mavlink_image_trigger_control_t; - - /** * @brief Pack a image_trigger_control message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_image_trigger_control_t */ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enable) { - uint16_t i = 0; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enable) { - uint16_t i = 0; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i * @param enable 0 to disable, 1 to enable */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { mavlink_message_t msg; - mavlink_msg_image_trigger_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enable); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg.payload[0]; + + p->enable = enable; // uint8_t:0 to disable, 1 to enable + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_image_trigger_control_t *p = &payload; + + p->enable = enable; // uint8_t:0 to disable, 1 to enable + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,7 +131,8 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; + return (uint8_t)(p->enable); } /** @@ -97,5 +143,5 @@ static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink */ static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control) { - image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); + memcpy( image_trigger_control, msg->payload, sizeof(mavlink_image_trigger_control_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 000003f3d7..6087d35231 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_TRIGGERED PACKING #define MAVLINK_MSG_ID_IMAGE_TRIGGERED 101 +#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 +#define MAVLINK_MSG_101_LEN 52 typedef struct __mavlink_image_triggered_t { @@ -19,8 +21,6 @@ typedef struct __mavlink_image_triggered_t } mavlink_image_triggered_t; - - /** * @brief Pack a image_triggered message * @param system_id ID of this system @@ -43,23 +43,23 @@ typedef struct __mavlink_image_triggered_t */ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); + 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); } /** @@ -84,23 +84,23 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + 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); } /** @@ -134,12 +134,79 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_image_triggered_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; + msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_image_triggered_t *p = &payload; + + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -152,16 +219,8 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (uint64_t)(p->timestamp); } /** @@ -171,12 +230,8 @@ static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_m */ static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (uint32_t)r.i; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (uint32_t)(p->seq); } /** @@ -186,12 +241,8 @@ static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message */ static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +252,8 @@ static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t */ static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +263,8 @@ static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_ */ static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -231,12 +274,8 @@ static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->local_z); } /** @@ -246,12 +285,8 @@ static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_messag */ static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -261,12 +296,8 @@ static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -276,12 +307,8 @@ static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -291,12 +318,8 @@ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_x); } /** @@ -306,12 +329,8 @@ static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_messa */ static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_y); } /** @@ -321,12 +340,8 @@ static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_messa */ static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_z); } /** @@ -337,16 +352,5 @@ static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_messa */ static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* msg, mavlink_image_triggered_t* image_triggered) { - image_triggered->timestamp = mavlink_msg_image_triggered_get_timestamp(msg); - image_triggered->seq = mavlink_msg_image_triggered_get_seq(msg); - image_triggered->roll = mavlink_msg_image_triggered_get_roll(msg); - image_triggered->pitch = mavlink_msg_image_triggered_get_pitch(msg); - image_triggered->yaw = mavlink_msg_image_triggered_get_yaw(msg); - image_triggered->local_z = mavlink_msg_image_triggered_get_local_z(msg); - image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg); - image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg); - image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg); - image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg); - image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg); - image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg); + memcpy( image_triggered, msg->payload, sizeof(mavlink_image_triggered_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index fb275534ae..e5519ef6df 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -1,6 +1,8 @@ // MESSAGE MARKER PACKING #define MAVLINK_MSG_ID_MARKER 130 +#define MAVLINK_MSG_ID_MARKER_LEN 26 +#define MAVLINK_MSG_130_LEN 26 typedef struct __mavlink_marker_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_marker_t } mavlink_marker_t; - - /** * @brief Pack a marker message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_marker_t */ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - i += put_uint16_t_by_index(id, i, msg->payload); // ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon */ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - i += put_uint16_t_by_index(id, i, msg->payload); // ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp * @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_message_t msg; - mavlink_msg_marker_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_marker_t *p = (mavlink_marker_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_MARKER_LEN; + msg.msgid = MAVLINK_MSG_ID_MARKER; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_marker_t *p = &payload; + + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MARKER_LEN; + hdr.msgid = MAVLINK_MSG_ID_MARKER; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,10 +179,8 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, */ static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -135,12 +190,8 @@ static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -150,12 +201,8 @@ static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -165,12 +212,8 @@ static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -180,12 +223,8 @@ static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -195,12 +234,8 @@ static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -210,12 +245,8 @@ static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -226,11 +257,5 @@ static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) */ static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavlink_marker_t* marker) { - marker->id = mavlink_msg_marker_get_id(msg); - marker->x = mavlink_msg_marker_get_x(msg); - marker->y = mavlink_msg_marker_get_y(msg); - marker->z = mavlink_msg_marker_get_z(msg); - marker->roll = mavlink_msg_marker_get_roll(msg); - marker->pitch = mavlink_msg_marker_get_pitch(msg); - marker->yaw = mavlink_msg_marker_get_yaw(msg); + memcpy( marker, msg->payload, sizeof(mavlink_marker_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 1a2b46596b..311e3be974 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -1,19 +1,19 @@ // MESSAGE PATTERN_DETECTED PACKING #define MAVLINK_MSG_ID_PATTERN_DETECTED 160 +#define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106 +#define MAVLINK_MSG_160_LEN 106 typedef struct __mavlink_pattern_detected_t { uint8_t type; ///< 0: Pattern, 1: Letter float confidence; ///< Confidence of detection - int8_t file[100]; ///< Pattern file name + char file[100]; ///< Pattern file name uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; - #define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 - /** * @brief Pack a pattern_detected message * @param system_id ID of this system @@ -26,17 +26,17 @@ typedef struct __mavlink_pattern_detected_t * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { - uint16_t i = 0; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { - uint16_t i = 0; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param detected Accepted as true detection, 0 no, 1 yes */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) { mavlink_message_t msg; - mavlink_msg_pattern_detected_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, confidence, file, detected); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; + msg.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_pattern_detected_t *p = &payload; + + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; + hdr.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -115,12 +167,8 @@ static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_messag */ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (float)(p->confidence); } /** @@ -128,11 +176,12 @@ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_me * * @return Pattern file name */ -static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char* file) { + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(float), 100); - return 100; + memcpy(file, p->file, sizeof(p->file)); + return sizeof(p->file); } /** @@ -142,7 +191,8 @@ static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_messa */ static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+100)[0]; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (uint8_t)(p->detected); } /** @@ -153,8 +203,5 @@ static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_me */ static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* msg, mavlink_pattern_detected_t* pattern_detected) { - pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); - pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); - mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); - pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); + memcpy( pattern_detected, msg->payload, sizeof(mavlink_pattern_detected_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h index 7e48c0fd91..284bd4a21b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -1,6 +1,8 @@ // MESSAGE POINT_OF_INTEREST PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 42 +#define MAVLINK_MSG_161_LEN 42 typedef struct __mavlink_point_of_interest_t { @@ -11,13 +13,11 @@ typedef struct __mavlink_point_of_interest_t float x; ///< X Position float y; ///< Y Position float z; ///< Z Position - int8_t name[25]; ///< POI name + char name[25]; ///< POI name } mavlink_point_of_interest_t; - #define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 25 - /** * @brief Pack a point_of_interest message * @param system_id ID of this system @@ -34,21 +34,21 @@ typedef struct __mavlink_point_of_interest_t * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_array_by_index(name, 25, i, msg->payload); // POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } /** @@ -67,21 +67,21 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_array_by_index(name, 25, i, msg->payload); // POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } /** @@ -111,12 +111,71 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param name POI name */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { mavlink_message_t msg; - mavlink_msg_point_of_interest_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, x, y, z, name); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg.payload[0]; + + 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[25]:POI name + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; + msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_point_of_interest_t *p = &payload; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; + hdr.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -129,7 +188,8 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui */ static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -139,7 +199,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_messa */ static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->color); } /** @@ -149,7 +210,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_mess */ static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->coordinate_system); } /** @@ -159,10 +221,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const */ static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint16_t)(p->timeout); } /** @@ -172,12 +232,8 @@ static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_m */ static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -187,12 +243,8 @@ static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* */ static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -202,12 +254,8 @@ static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* */ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -215,11 +263,12 @@ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* * * @return POI name */ -static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char* name) { + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -230,12 +279,5 @@ static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_mess */ static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* msg, mavlink_point_of_interest_t* point_of_interest) { - point_of_interest->type = mavlink_msg_point_of_interest_get_type(msg); - point_of_interest->color = mavlink_msg_point_of_interest_get_color(msg); - point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg); - point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); - point_of_interest->x = mavlink_msg_point_of_interest_get_x(msg); - point_of_interest->y = mavlink_msg_point_of_interest_get_y(msg); - point_of_interest->z = mavlink_msg_point_of_interest_get_z(msg); - mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); + memcpy( point_of_interest, msg->payload, sizeof(mavlink_point_of_interest_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h index fb30517463..9724506927 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,6 +1,8 @@ // MESSAGE POINT_OF_INTEREST_CONNECTION PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 54 +#define MAVLINK_MSG_162_LEN 54 typedef struct __mavlink_point_of_interest_connection_t { @@ -14,13 +16,11 @@ typedef struct __mavlink_point_of_interest_connection_t float xp2; ///< X2 Position float yp2; ///< Y2 Position float zp2; ///< Z2 Position - int8_t name[25]; ///< POI connection name + char name[25]; ///< POI connection name } mavlink_point_of_interest_connection_t; - #define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 25 - /** * @brief Pack a point_of_interest_connection message * @param system_id ID of this system @@ -40,24 +40,24 @@ typedef struct __mavlink_point_of_interest_connection_t * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name - - return mavlink_finalize_message(msg, system_id, component_id, i); + 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[25]:POI connection name + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } /** @@ -79,24 +79,24 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + 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[25]:POI connection name + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } /** @@ -129,12 +129,77 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param name POI connection name */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { mavlink_message_t msg; - mavlink_msg_point_of_interest_connection_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, xp1, yp1, zp1, xp2, yp2, zp2, name); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg.payload[0]; + + 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[25]:POI connection name + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; + msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_point_of_interest_connection_t *p = &payload; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -147,7 +212,8 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -157,7 +223,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const ma */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->color); } /** @@ -167,7 +234,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const m */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->coordinate_system); } /** @@ -177,10 +245,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_sy */ static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint16_t)(p->timeout); } /** @@ -190,12 +256,8 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(cons */ static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->xp1); } /** @@ -205,12 +267,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->yp1); } /** @@ -220,12 +278,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->zp1); } /** @@ -235,12 +289,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->xp2); } /** @@ -250,12 +300,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->yp2); } /** @@ -265,12 +311,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->zp2); } /** @@ -278,11 +320,12 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavli * * @return POI connection name */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char* name) { + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -293,15 +336,5 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const m */ static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink_message_t* msg, mavlink_point_of_interest_connection_t* point_of_interest_connection) { - point_of_interest_connection->type = mavlink_msg_point_of_interest_connection_get_type(msg); - point_of_interest_connection->color = mavlink_msg_point_of_interest_connection_get_color(msg); - point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg); - point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); - point_of_interest_connection->xp1 = mavlink_msg_point_of_interest_connection_get_xp1(msg); - point_of_interest_connection->yp1 = mavlink_msg_point_of_interest_connection_get_yp1(msg); - point_of_interest_connection->zp1 = mavlink_msg_point_of_interest_connection_get_zp1(msg); - point_of_interest_connection->xp2 = mavlink_msg_point_of_interest_connection_get_xp2(msg); - point_of_interest_connection->yp2 = mavlink_msg_point_of_interest_connection_get_yp2(msg); - point_of_interest_connection->zp2 = mavlink_msg_point_of_interest_connection_get_zp2(msg); - mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); + memcpy( point_of_interest_connection, msg->payload, sizeof(mavlink_point_of_interest_connection_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h index 30f076bba8..2b508a62a5 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,6 +1,8 @@ // MESSAGE POSITION_CONTROL_OFFSET_SET PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 154 +#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN 18 +#define MAVLINK_MSG_154_LEN 18 typedef struct __mavlink_position_control_offset_set_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_position_control_offset_set_t } mavlink_position_control_offset_set_t; - - /** * @brief Pack a position_control_offset_set message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_position_control_offset_set_t */ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t syst */ static inline uint16_t mavlink_msg_position_control_offset_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy * @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_message_t msg; - mavlink_msg_position_control_offset_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_position_control_offset_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +171,8 @@ static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -126,7 +182,8 @@ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system( */ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -136,12 +193,8 @@ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_compone */ static inline float mavlink_msg_position_control_offset_set_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -151,12 +204,8 @@ static inline float mavlink_msg_position_control_offset_set_get_x(const mavlink_ */ static inline float mavlink_msg_position_control_offset_set_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -166,12 +215,8 @@ static inline float mavlink_msg_position_control_offset_set_get_y(const mavlink_ */ static inline float mavlink_msg_position_control_offset_set_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -181,12 +226,8 @@ static inline float mavlink_msg_position_control_offset_set_get_z(const mavlink_ */ static inline float mavlink_msg_position_control_offset_set_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -197,10 +238,5 @@ static inline float mavlink_msg_position_control_offset_set_get_yaw(const mavlin */ static inline void mavlink_msg_position_control_offset_set_decode(const mavlink_message_t* msg, mavlink_position_control_offset_set_t* position_control_offset_set) { - position_control_offset_set->target_system = mavlink_msg_position_control_offset_set_get_target_system(msg); - position_control_offset_set->target_component = mavlink_msg_position_control_offset_set_get_target_component(msg); - position_control_offset_set->x = mavlink_msg_position_control_offset_set_get_x(msg); - position_control_offset_set->y = mavlink_msg_position_control_offset_set_get_y(msg); - position_control_offset_set->z = mavlink_msg_position_control_offset_set_get_z(msg); - position_control_offset_set->yaw = mavlink_msg_position_control_offset_set_get_yaw(msg); + memcpy( position_control_offset_set, msg->payload, sizeof(mavlink_position_control_offset_set_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h index 59bcb131ce..ee33580eb9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROL_SETPOINT PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 121 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 +#define MAVLINK_MSG_121_LEN 18 typedef struct __mavlink_position_control_setpoint_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_position_control_setpoint_t } mavlink_position_control_setpoint_t; - - /** * @brief Pack a position_control_setpoint message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_position_control_setpoint_t */ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system */ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst * @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_message_t msg; - mavlink_msg_position_control_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_position_control_setpoint_t *p = &payload; + + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,10 +163,8 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t */ static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -123,12 +174,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlin */ static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -138,12 +185,8 @@ static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -153,12 +196,8 @@ static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -168,12 +207,8 @@ static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -184,9 +219,5 @@ static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_ */ static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_t* position_control_setpoint) { - position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); - position_control_setpoint->x = mavlink_msg_position_control_setpoint_get_x(msg); - position_control_setpoint->y = mavlink_msg_position_control_setpoint_get_y(msg); - position_control_setpoint->z = mavlink_msg_position_control_setpoint_get_z(msg); - position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg); + memcpy( position_control_setpoint, msg->payload, sizeof(mavlink_position_control_setpoint_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h index 75150e66b9..031a73b01b 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,6 +1,8 @@ // MESSAGE POSITION_CONTROL_SETPOINT_SET PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 120 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN 20 +#define MAVLINK_MSG_120_LEN 20 typedef struct __mavlink_position_control_setpoint_set_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_position_control_setpoint_set_t } mavlink_position_control_setpoint_set_t; - - /** * @brief Pack a position_control_setpoint_set message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_position_control_setpoint_set_t */ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t sy */ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t * @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_message_t msg; - mavlink_msg_position_control_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_position_control_setpoint_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channe */ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -132,7 +190,8 @@ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_syste */ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -142,10 +201,8 @@ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_compo */ static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -155,12 +212,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(const ma */ static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -170,12 +223,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -185,12 +234,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -200,12 +245,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -216,11 +257,5 @@ static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavl */ static inline void mavlink_msg_position_control_setpoint_set_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_set_t* position_control_setpoint_set) { - position_control_setpoint_set->target_system = mavlink_msg_position_control_setpoint_set_get_target_system(msg); - position_control_setpoint_set->target_component = mavlink_msg_position_control_setpoint_set_get_target_component(msg); - position_control_setpoint_set->id = mavlink_msg_position_control_setpoint_set_get_id(msg); - position_control_setpoint_set->x = mavlink_msg_position_control_setpoint_set_get_x(msg); - position_control_setpoint_set->y = mavlink_msg_position_control_setpoint_set_get_y(msg); - position_control_setpoint_set->z = mavlink_msg_position_control_setpoint_set_get_z(msg); - position_control_setpoint_set->yaw = mavlink_msg_position_control_setpoint_set_get_yaw(msg); + memcpy( position_control_setpoint_set, msg->payload, sizeof(mavlink_position_control_setpoint_set_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 61adee2970..32b82e5686 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -1,6 +1,8 @@ // MESSAGE RAW_AUX PACKING #define MAVLINK_MSG_ID_RAW_AUX 141 +#define MAVLINK_MSG_ID_RAW_AUX_LEN 16 +#define MAVLINK_MSG_141_LEN 16 typedef struct __mavlink_raw_aux_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_raw_aux_t } mavlink_raw_aux_t; - - /** * @brief Pack a raw_aux message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_raw_aux_t */ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - uint16_t i = 0; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - uint16_t i = 0; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com * @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_message_t msg; - mavlink_msg_raw_aux_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, adc1, adc2, adc3, adc4, vbat, temp, baro); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RAW_AUX_LEN; + msg.msgid = MAVLINK_MSG_ID_RAW_AUX; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_raw_aux_t *p = &payload; + + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_AUX_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_AUX; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,10 +179,8 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc */ static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc1); } /** @@ -135,10 +190,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc2); } /** @@ -148,10 +201,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc3); } /** @@ -161,10 +212,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc4); } /** @@ -174,10 +223,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->vbat); } /** @@ -187,10 +234,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (int16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (int16_t)(p->temp); } /** @@ -200,12 +245,8 @@ static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) */ static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[3]; - return (int32_t)r.i; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (int32_t)(p->baro); } /** @@ -216,11 +257,5 @@ static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) */ static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavlink_raw_aux_t* raw_aux) { - raw_aux->adc1 = mavlink_msg_raw_aux_get_adc1(msg); - raw_aux->adc2 = mavlink_msg_raw_aux_get_adc2(msg); - raw_aux->adc3 = mavlink_msg_raw_aux_get_adc3(msg); - raw_aux->adc4 = mavlink_msg_raw_aux_get_adc4(msg); - raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg); - raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg); - raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); + memcpy( raw_aux, msg->payload, sizeof(mavlink_raw_aux_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h index 6a34b7e4bd..5db1aa478b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -1,6 +1,8 @@ // MESSAGE SET_CAM_SHUTTER PACKING #define MAVLINK_MSG_ID_SET_CAM_SHUTTER 100 +#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 +#define MAVLINK_MSG_100_LEN 11 typedef struct __mavlink_set_cam_shutter_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_set_cam_shutter_t } mavlink_set_cam_shutter_t; - - /** * @brief Pack a set_cam_shutter message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_set_cam_shutter_t */ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - uint16_t i = 0; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - uint16_t i = 0; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_set_cam_shutter_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_no, cam_mode, trigger_pin, interval, exposure, gain); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_set_cam_shutter_t *p = &payload; + + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +171,8 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->cam_no); } /** @@ -126,7 +182,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_messa */ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->cam_mode); } /** @@ -136,7 +193,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_mes */ static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->trigger_pin); } /** @@ -146,10 +204,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_ */ static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint16_t)(p->interval); } /** @@ -159,10 +215,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_me */ static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint16_t)(p->exposure); } /** @@ -172,12 +226,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_me */ static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (float)(p->gain); } /** @@ -188,10 +238,5 @@ static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t */ static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* msg, mavlink_set_cam_shutter_t* set_cam_shutter) { - set_cam_shutter->cam_no = mavlink_msg_set_cam_shutter_get_cam_no(msg); - set_cam_shutter->cam_mode = mavlink_msg_set_cam_shutter_get_cam_mode(msg); - set_cam_shutter->trigger_pin = mavlink_msg_set_cam_shutter_get_trigger_pin(msg); - set_cam_shutter->interval = mavlink_msg_set_cam_shutter_get_interval(msg); - set_cam_shutter->exposure = mavlink_msg_set_cam_shutter_get_exposure(msg); - set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(msg); + memcpy( set_cam_shutter, msg->payload, sizeof(mavlink_set_cam_shutter_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h index 6e349eafc5..c27e22bc2b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VICON_POSITION_ESTIMATE PACKING #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 112 +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_112_LEN 32 typedef struct __mavlink_vicon_position_estimate_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_vicon_position_estimate_t } mavlink_vicon_position_estimate_t; - - /** * @brief Pack a vicon_position_estimate message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_vicon_position_estimate_t */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system * @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_message_t msg; - mavlink_msg_vicon_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; + msg.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_vicon_position_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch */ static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlin */ static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_m */ static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_ */ static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_me */ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) { - vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); - vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); - vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); - vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); - vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); - vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); - vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); + memcpy( vicon_position_estimate, msg->payload, sizeof(mavlink_vicon_position_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h index 30728191d4..dbf29c50ab 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VISION_POSITION_ESTIMATE PACKING #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 111 +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_111_LEN 32 typedef struct __mavlink_vision_position_estimate_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_vision_position_estimate_t } mavlink_vision_position_estimate_t; - - /** * @brief Pack a vision_position_estimate message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_vision_position_estimate_t */ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ */ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste * @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_message_t msg; - mavlink_msg_vision_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; + msg.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_vision_position_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c */ static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavli */ static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_ */ static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink */ static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_m */ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) { - vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); - vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); - vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); - vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); - vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); - vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); - vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); + memcpy( vision_position_estimate, msg->payload, sizeof(mavlink_vision_position_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h index 66224c28e9..569f7123b9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VISION_SPEED_ESTIMATE PACKING #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113 +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_113_LEN 20 typedef struct __mavlink_vision_speed_estimate_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_vision_speed_estimate_t } mavlink_vision_speed_estimate_t; - - /** * @brief Pack a vision_speed_estimate message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_vision_speed_estimate_t */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i * @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_message_t msg; - mavlink_msg_vision_speed_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; + msg.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_vision_speed_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,16 +155,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan */ static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -123,12 +166,8 @@ static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_ */ static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -138,12 +177,8 @@ static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_messag */ static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -153,12 +188,8 @@ static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_messag */ static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -169,8 +200,5 @@ static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_messag */ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) { - vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); - vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); - vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); - vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); + memcpy( vision_speed_estimate, msg->payload, sizeof(mavlink_vision_speed_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index 25ea0f5f1d..a039a8acaa 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_COMMAND PACKING #define MAVLINK_MSG_ID_WATCHDOG_COMMAND 153 +#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 +#define MAVLINK_MSG_153_LEN 6 typedef struct __mavlink_watchdog_command_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_watchdog_command_t } mavlink_watchdog_command_t; - - /** * @brief Pack a watchdog_command message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_watchdog_command_t */ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - uint16_t i = 0; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - uint16_t i = 0; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui * @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_message_t msg; - mavlink_msg_watchdog_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system_id, watchdog_id, process_id, command_id); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_watchdog_command_t *p = &payload; + + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,7 +155,8 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_system_id); } /** @@ -114,10 +166,8 @@ static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const ma */ static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -127,10 +177,8 @@ static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlin */ static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -140,7 +188,8 @@ static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink */ static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint8_t)(p->command_id); } /** @@ -151,8 +200,5 @@ static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_ */ static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* msg, mavlink_watchdog_command_t* watchdog_command) { - watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); - watchdog_command->watchdog_id = mavlink_msg_watchdog_command_get_watchdog_id(msg); - watchdog_command->process_id = mavlink_msg_watchdog_command_get_process_id(msg); - watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); + memcpy( watchdog_command, msg->payload, sizeof(mavlink_watchdog_command_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index db693bc554..4d5c2687e4 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_HEARTBEAT PACKING #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 150 +#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 +#define MAVLINK_MSG_150_LEN 4 typedef struct __mavlink_watchdog_heartbeat_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_watchdog_heartbeat_t } mavlink_watchdog_heartbeat_t; - - /** * @brief Pack a watchdog_heartbeat message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_watchdog_heartbeat_t */ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_count) { - uint16_t i = 0; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_count) { - uint16_t i = 0; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_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_message_t msg; - mavlink_msg_watchdog_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_count); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg.payload[0]; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_watchdog_heartbeat_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,10 +139,8 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u */ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -105,10 +150,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavl */ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; + return (uint16_t)(p->process_count); } /** @@ -119,6 +162,5 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const ma */ static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat) { - watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); - watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); + memcpy( watchdog_heartbeat, msg->payload, sizeof(mavlink_watchdog_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h index a312a14679..44a5cf2434 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -1,21 +1,21 @@ // MESSAGE WATCHDOG_PROCESS_INFO PACKING #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 151 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 +#define MAVLINK_MSG_151_LEN 255 typedef struct __mavlink_watchdog_process_info_t { uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID - int8_t name[100]; ///< Process name - int8_t arguments[147]; ///< Process arguments + char name[100]; ///< Process name + char arguments[147]; ///< Process arguments int32_t timeout; ///< Timeout (seconds) } mavlink_watchdog_process_info_t; - #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 - /** * @brief Pack a watchdog_process_info message * @param system_id ID of this system @@ -29,18 +29,18 @@ typedef struct __mavlink_watchdog_process_info_t * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - uint16_t i = 0; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } /** @@ -56,18 +56,18 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - uint16_t i = 0; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } /** @@ -94,12 +94,65 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param timeout Timeout (seconds) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { mavlink_message_t msg; - mavlink_msg_watchdog_process_info_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, name, arguments, timeout); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_watchdog_process_info_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -112,10 +165,8 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan */ static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -125,10 +176,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const m */ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -136,11 +185,12 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const ma * * @return Process name */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char* name) { + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t), 100); - return 100; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -148,11 +198,12 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_ * * @return Process arguments */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char* arguments) { + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100, 147); - return 147; + memcpy(arguments, p->arguments, sizeof(p->arguments)); + return sizeof(p->arguments); } /** @@ -162,12 +213,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mav */ static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[3]; - return (int32_t)r.i; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (int32_t)(p->timeout); } /** @@ -178,9 +225,5 @@ static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlin */ static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_message_t* msg, mavlink_watchdog_process_info_t* watchdog_process_info) { - watchdog_process_info->watchdog_id = mavlink_msg_watchdog_process_info_get_watchdog_id(msg); - watchdog_process_info->process_id = mavlink_msg_watchdog_process_info_get_process_id(msg); - mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name); - mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments); - watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); + memcpy( watchdog_process_info, msg->payload, sizeof(mavlink_watchdog_process_info_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h index 9afa6ca14a..01c80eb317 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_PROCESS_STATUS PACKING #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 152 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 +#define MAVLINK_MSG_152_LEN 12 typedef struct __mavlink_watchdog_process_status_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_watchdog_process_status_t } mavlink_watchdog_process_status_t; - - /** * @brief Pack a watchdog_process_status message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_watchdog_process_status_t */ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - uint16_t i = 0; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - uint16_t i = 0; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system * @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_message_t msg; - mavlink_msg_watchdog_process_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, state, muted, pid, crashes); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_watchdog_process_status_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,10 +171,8 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch */ static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -129,10 +182,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const */ static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -142,7 +193,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const */ static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint8_t)(p->state); } /** @@ -152,7 +204,8 @@ static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlin */ static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint8_t)(p->muted); } /** @@ -162,12 +215,8 @@ static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlin */ static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (int32_t)(p->pid); } /** @@ -177,10 +226,8 @@ static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_ */ static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->crashes); } /** @@ -191,10 +238,5 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mav */ static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_message_t* msg, mavlink_watchdog_process_status_t* watchdog_process_status) { - watchdog_process_status->watchdog_id = mavlink_msg_watchdog_process_status_get_watchdog_id(msg); - watchdog_process_status->process_id = mavlink_msg_watchdog_process_status_get_process_id(msg); - watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg); - watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); - watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); - watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg); + memcpy( watchdog_process_status, msg->payload, sizeof(mavlink_watchdog_process_status_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 30474d09cb..618dbe87c8 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -30,7 +30,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Content Types for data transmission handshake */ +/** @brief Content Types for data transmission handshake */ enum DATA_TYPES { DATA_TYPE_JPEG_IMAGE=1, @@ -66,13 +66,6 @@ enum DATA_TYPES #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" #include "./mavlink_msg_brief_feature.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/protocol.h b/thirdParty/mavlink/include/protocol.h index ed1f9546b4..e9cca2e22c 100644 --- a/thirdParty/mavlink/include/protocol.h +++ b/thirdParty/mavlink/include/protocol.h @@ -2,10 +2,13 @@ #define _MAVLINK_PROTOCOL_H_ #include "string.h" -#include "checksum.h" - #include "mavlink_types.h" +#include "checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t mavlink_msg_lengths[256]; +#endif /** * @brief Initialize the communication stack @@ -34,7 +37,12 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +#endif + return &m_mavlink_status[chan]; } @@ -68,8 +76,9 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t msg->compid = component_id; // One sequence number per component msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -98,7 +107,8 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -111,11 +121,13 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) { *(buffer+0) = MAVLINK_STX; ///< Start transmit - memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - return 0; +// return 0; } /** @@ -197,8 +209,13 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) */ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +#elif defined(NB_MAVLINK_COMM) + static mavlink_message_t m_mavlink_message[NB_MAVLINK_COMM]; +#else + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +#endif // Initializes only once, values keep unchanged after first initialization mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); @@ -258,6 +275,11 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag case MAVLINK_PARSE_STATE_GOT_COMPID: rxmsg->msgid = c; mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != mavlink_msg_lengths[c] ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif if (rxmsg->len == 0) { status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; @@ -313,7 +335,9 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag // Successfully got message status->msg_received = 1; status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; } break; } @@ -338,10 +362,86 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; r_mavlink_status->packet_rx_drop_count = status->parse_error; status->parse_error = 0; + + // 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 +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#define FILE_FINISHED + +#ifndef FILE_FINISHED /** * 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 @@ -383,13 +483,23 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag /* static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS]; + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; + #else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; + #endif // Set a packet start candidate index if sign is start sign if (c == MAVLINK_STX) @@ -399,8 +509,13 @@ static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_me // Parse normally, if a CRC mismatch occurs retry with the next packet index } -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; +//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +//#else +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +//#endif //// Initializes only once, values keep unchanged after first initialization // mavlink_parse_state_initialize(&m_mavlink_status[chan]); // @@ -552,7 +667,6 @@ typedef union __generic_64bit { uint8_t b[8]; int64_t ll; ///< Long long (64 bit) - double d; ///< IEEE-754 double precision floating point } generic_64bit; /** @@ -839,144 +953,7 @@ static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t p 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 a similar function - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum) -{ - crc_accumulate(b, checksum); - comm_send_ch(chan, b); -} - -static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum) -{ - crc_accumulate(b, checksum); - comm_send_ch(chan, b); -} - -static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum) -{ - char s; - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint16_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum) -{ - char s; - s = (b>>24)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>16)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint32_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum) -{ - char s; - s = (b>>56)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>48)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>40)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>32)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>24)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>16)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint64_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum) -{ - generic_32bit g; - g.f = b; - mavlink_send_uart_uint32_t(chan, g.i, checksum); -} - -static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum) -{ - generic_64bit g; - g.d = b; - mavlink_send_uart_uint32_t(chan, g.ll, checksum); -} - -static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) -{ - // 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); -} -#endif +*/ +#endif /* FILE_FINISHED */ #endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index bf0e146b4c..1379932f27 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "slugs.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 2b212ae1ba..19503c1df2 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -1,6 +1,8 @@ // MESSAGE AIR_DATA PACKING #define MAVLINK_MSG_ID_AIR_DATA 171 +#define MAVLINK_MSG_ID_AIR_DATA_LEN 10 +#define MAVLINK_MSG_171_LEN 10 typedef struct __mavlink_air_data_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_air_data_t } mavlink_air_data_t; - - /** * @brief Pack a air_data message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_air_data_t */ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) { - uint16_t i = 0; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIR_DATA_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) { - uint16_t i = 0; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIR_DATA_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co * @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_message_t msg; - mavlink_msg_air_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, dynamicPressure, staticPressure, temperature); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg.payload[0]; + + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_AIR_DATA_LEN; + msg.msgid = MAVLINK_MSG_ID_AIR_DATA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_air_data_t *p = &payload; + + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AIR_DATA_LEN; + hdr.msgid = MAVLINK_MSG_ID_AIR_DATA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,12 +147,8 @@ static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynam */ static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (float)(p->dynamicPressure); } /** @@ -113,12 +158,8 @@ static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_messa */ static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (float)(p->staticPressure); } /** @@ -128,10 +169,8 @@ static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_messag */ static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (uint16_t)r.s; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (uint16_t)(p->temperature); } /** @@ -142,7 +181,5 @@ static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_messag */ static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mavlink_air_data_t* air_data) { - air_data->dynamicPressure = mavlink_msg_air_data_get_dynamicPressure(msg); - air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg); - air_data->temperature = mavlink_msg_air_data_get_temperature(msg); + memcpy( air_data, msg->payload, sizeof(mavlink_air_data_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index 265aa3fcee..d3191546a1 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -1,6 +1,8 @@ // MESSAGE CPU_LOAD PACKING #define MAVLINK_MSG_ID_CPU_LOAD 170 +#define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 +#define MAVLINK_MSG_170_LEN 4 typedef struct __mavlink_cpu_load_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_cpu_load_t } mavlink_cpu_load_t; - - /** * @brief Pack a cpu_load message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_cpu_load_t */ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - uint16_t i = 0; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CPU_LOAD_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - uint16_t i = 0; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CPU_LOAD_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co * @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_message_t msg; - mavlink_msg_cpu_load_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, sensLoad, ctrlLoad, batVolt); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; + msg.msgid = MAVLINK_MSG_ID_CPU_LOAD; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_cpu_load_t *p = &payload; + + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; + hdr.msgid = MAVLINK_MSG_ID_CPU_LOAD; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sen */ static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint8_t)(p->sensLoad); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* */ static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint8_t)(p->ctrlLoad); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* */ static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint16_t)(p->batVolt); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* */ static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mavlink_cpu_load_t* cpu_load) { - cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); - cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); - cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); + memcpy( cpu_load, msg->payload, sizeof(mavlink_cpu_load_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h index 9221dc4f00..f983a544cf 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -1,6 +1,8 @@ // MESSAGE CTRL_SRFC_PT PACKING #define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 +#define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 +#define MAVLINK_MSG_181_LEN 3 typedef struct __mavlink_ctrl_srfc_pt_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_ctrl_srfc_pt_t } mavlink_ctrl_srfc_pt_t; - - /** * @brief Pack a ctrl_srfc_pt message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_ctrl_srfc_pt_t */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) { - uint16_t i = 0; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) { - uint16_t i = 0; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_ctrl_srfc_pt_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, bitfieldPt); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; + msg.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_ctrl_srfc_pt_t *p = &payload; + + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; + hdr.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,10 +150,8 @@ static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_ */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; + return (uint16_t)(p->bitfieldPt); } /** @@ -116,6 +162,5 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_mes */ static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) { - ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); - ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); + memcpy( ctrl_srfc_pt, msg->payload, sizeof(mavlink_ctrl_srfc_pt_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index cbcc0daba3..018de9224d 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -1,6 +1,8 @@ // MESSAGE DATA_LOG PACKING #define MAVLINK_MSG_ID_DATA_LOG 177 +#define MAVLINK_MSG_ID_DATA_LOG_LEN 24 +#define MAVLINK_MSG_177_LEN 24 typedef struct __mavlink_data_log_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_data_log_t } mavlink_data_log_t; - - /** * @brief Pack a data_log message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_data_log_t */ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - uint16_t i = 0; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_LOG_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - uint16_t i = 0; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_LOG_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co * @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_message_t msg; - mavlink_msg_data_log_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, fl_1, fl_2, fl_3, fl_4, fl_5, fl_6); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DATA_LOG_LEN; + msg.msgid = MAVLINK_MSG_ID_DATA_LOG; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_data_log_t *p = &payload; + + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DATA_LOG_LEN; + hdr.msgid = MAVLINK_MSG_ID_DATA_LOG; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, */ static inline float mavlink_msg_data_log_get_fl_1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_1); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_data_log_get_fl_1(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_2); } /** @@ -146,12 +193,8 @@ static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_3); } /** @@ -161,12 +204,8 @@ static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_4); } /** @@ -176,12 +215,8 @@ static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_5); } /** @@ -191,12 +226,8 @@ static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_6); } /** @@ -207,10 +238,5 @@ static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) */ static inline void mavlink_msg_data_log_decode(const mavlink_message_t* msg, mavlink_data_log_t* data_log) { - data_log->fl_1 = mavlink_msg_data_log_get_fl_1(msg); - data_log->fl_2 = mavlink_msg_data_log_get_fl_2(msg); - data_log->fl_3 = mavlink_msg_data_log_get_fl_3(msg); - data_log->fl_4 = mavlink_msg_data_log_get_fl_4(msg); - data_log->fl_5 = mavlink_msg_data_log_get_fl_5(msg); - data_log->fl_6 = mavlink_msg_data_log_get_fl_6(msg); + memcpy( data_log, msg->payload, sizeof(mavlink_data_log_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index f3798f59bb..4bfebe5fb4 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -1,6 +1,8 @@ // MESSAGE DIAGNOSTIC PACKING #define MAVLINK_MSG_ID_DIAGNOSTIC 173 +#define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 +#define MAVLINK_MSG_173_LEN 18 typedef struct __mavlink_diagnostic_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_diagnostic_t } mavlink_diagnostic_t; - - /** * @brief Pack a diagnostic message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_diagnostic_t */ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - uint16_t i = 0; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - uint16_t i = 0; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t * @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_message_t msg; - mavlink_msg_diagnostic_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, diagFl1, diagFl2, diagFl3, diagSh1, diagSh2, diagSh3); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; + msg.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_diagnostic_t *p = &payload; + + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; + hdr.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float dia */ static inline float mavlink_msg_diagnostic_get_diagFl1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl1); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl1(const mavlink_message_t* */ static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl2); } /** @@ -146,12 +193,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* */ static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl3); } /** @@ -161,10 +204,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* */ static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh1); } /** @@ -174,10 +215,8 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t */ static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh2); } /** @@ -187,10 +226,8 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t */ static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh3); } /** @@ -201,10 +238,5 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t */ static inline void mavlink_msg_diagnostic_decode(const mavlink_message_t* msg, mavlink_diagnostic_t* diagnostic) { - diagnostic->diagFl1 = mavlink_msg_diagnostic_get_diagFl1(msg); - diagnostic->diagFl2 = mavlink_msg_diagnostic_get_diagFl2(msg); - diagnostic->diagFl3 = mavlink_msg_diagnostic_get_diagFl3(msg); - diagnostic->diagSh1 = mavlink_msg_diagnostic_get_diagSh1(msg); - diagnostic->diagSh2 = mavlink_msg_diagnostic_get_diagSh2(msg); - diagnostic->diagSh3 = mavlink_msg_diagnostic_get_diagSh3(msg); + memcpy( diagnostic, msg->payload, sizeof(mavlink_diagnostic_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h index 199cbb9ec2..ca3b1a934e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -1,6 +1,8 @@ // MESSAGE GPS_DATE_TIME PACKING #define MAVLINK_MSG_ID_GPS_DATE_TIME 179 +#define MAVLINK_MSG_ID_GPS_DATE_TIME_LEN 7 +#define MAVLINK_MSG_179_LEN 7 typedef struct __mavlink_gps_date_time_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_gps_date_time_t } mavlink_gps_date_time_t; - - /** * @brief Pack a gps_date_time message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_gps_date_time_t */ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - uint16_t i = 0; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - uint16_t i = 0; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 * @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_message_t msg; - mavlink_msg_gps_date_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, year, month, day, hour, min, sec, visSat); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_gps_date_time_t *p = &payload; + + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_ */ static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->year); } /** @@ -132,7 +190,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t */ static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->month); } /** @@ -142,7 +201,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_ */ static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->day); } /** @@ -152,7 +212,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->hour); } /** @@ -162,7 +223,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t */ static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->min); } /** @@ -172,7 +234,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->sec); } /** @@ -182,7 +245,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->visSat); } /** @@ -193,11 +257,5 @@ static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message */ static inline void mavlink_msg_gps_date_time_decode(const mavlink_message_t* msg, mavlink_gps_date_time_t* gps_date_time) { - gps_date_time->year = mavlink_msg_gps_date_time_get_year(msg); - gps_date_time->month = mavlink_msg_gps_date_time_get_month(msg); - gps_date_time->day = mavlink_msg_gps_date_time_get_day(msg); - gps_date_time->hour = mavlink_msg_gps_date_time_get_hour(msg); - gps_date_time->min = mavlink_msg_gps_date_time_get_min(msg); - gps_date_time->sec = mavlink_msg_gps_date_time_get_sec(msg); - gps_date_time->visSat = mavlink_msg_gps_date_time_get_visSat(msg); + memcpy( gps_date_time, msg->payload, sizeof(mavlink_gps_date_time_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h index a0d78b1ceb..c154613328 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -1,6 +1,8 @@ // MESSAGE MID_LVL_CMDS PACKING #define MAVLINK_MSG_ID_MID_LVL_CMDS 180 +#define MAVLINK_MSG_ID_MID_LVL_CMDS_LEN 13 +#define MAVLINK_MSG_180_LEN 13 typedef struct __mavlink_mid_lvl_cmds_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_mid_lvl_cmds_t } mavlink_mid_lvl_cmds_t; - - /** * @brief Pack a mid_lvl_cmds message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_mid_lvl_cmds_t */ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) { - uint16_t i = 0; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) { - uint16_t i = 0; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_mid_lvl_cmds_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, hCommand, uCommand, rCommand); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; + msg.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_mid_lvl_cmds_t *p = &payload; + + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; + hdr.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,7 +155,8 @@ static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -114,12 +166,8 @@ static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->hCommand); } /** @@ -129,12 +177,8 @@ static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->uCommand); } /** @@ -144,12 +188,8 @@ static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->rCommand); } /** @@ -160,8 +200,5 @@ static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_ */ static inline void mavlink_msg_mid_lvl_cmds_decode(const mavlink_message_t* msg, mavlink_mid_lvl_cmds_t* mid_lvl_cmds) { - mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); - mid_lvl_cmds->hCommand = mavlink_msg_mid_lvl_cmds_get_hCommand(msg); - mid_lvl_cmds->uCommand = mavlink_msg_mid_lvl_cmds_get_uCommand(msg); - mid_lvl_cmds->rCommand = mavlink_msg_mid_lvl_cmds_get_rCommand(msg); + memcpy( mid_lvl_cmds, msg->payload, sizeof(mavlink_mid_lvl_cmds_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 8b40bdd67a..944ef8d41e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -1,6 +1,8 @@ // MESSAGE SENSOR_BIAS PACKING #define MAVLINK_MSG_ID_SENSOR_BIAS 172 +#define MAVLINK_MSG_ID_SENSOR_BIAS_LEN 24 +#define MAVLINK_MSG_172_LEN 24 typedef struct __mavlink_sensor_bias_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_sensor_bias_t } mavlink_sensor_bias_t; - - /** * @brief Pack a sensor_bias message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_sensor_bias_t */ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - uint16_t i = 0; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - uint16_t i = 0; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t * @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_message_t msg; - mavlink_msg_sensor_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, axBias, ayBias, azBias, gxBias, gyBias, gzBias); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg.payload[0]; + + 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) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; + msg.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_sensor_bias_t *p = &payload; + + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; + hdr.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float ax */ static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->axBias); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->ayBias); } /** @@ -146,12 +193,8 @@ static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->azBias); } /** @@ -161,12 +204,8 @@ static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gxBias); } /** @@ -176,12 +215,8 @@ static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gyBias); } /** @@ -191,12 +226,8 @@ static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gzBias); } /** @@ -207,10 +238,5 @@ static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* */ static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, mavlink_sensor_bias_t* sensor_bias) { - sensor_bias->axBias = mavlink_msg_sensor_bias_get_axBias(msg); - sensor_bias->ayBias = mavlink_msg_sensor_bias_get_ayBias(msg); - sensor_bias->azBias = mavlink_msg_sensor_bias_get_azBias(msg); - sensor_bias->gxBias = mavlink_msg_sensor_bias_get_gxBias(msg); - sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg); - sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg); + memcpy( sensor_bias, msg->payload, sizeof(mavlink_sensor_bias_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index 0664166567..d860666897 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -1,6 +1,8 @@ // MESSAGE SLUGS_ACTION PACKING #define MAVLINK_MSG_ID_SLUGS_ACTION 183 +#define MAVLINK_MSG_ID_SLUGS_ACTION_LEN 4 +#define MAVLINK_MSG_183_LEN 4 typedef struct __mavlink_slugs_action_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_slugs_action_t } mavlink_slugs_action_t; - - /** * @brief Pack a slugs_action message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_slugs_action_t */ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) { - uint16_t i = 0; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) { - uint16_t i = 0; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ * @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_message_t msg; - mavlink_msg_slugs_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, actionId, actionVal); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; + msg.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_slugs_action_t *p = &payload; + + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_ */ static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint8_t)(p->actionId); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_messag */ static inline uint16_t mavlink_msg_slugs_action_get_actionVal(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint16_t)(p->actionVal); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_slugs_action_get_actionVal(const mavlink_mess */ static inline void mavlink_msg_slugs_action_decode(const mavlink_message_t* msg, mavlink_slugs_action_t* slugs_action) { - slugs_action->target = mavlink_msg_slugs_action_get_target(msg); - slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); - slugs_action->actionVal = mavlink_msg_slugs_action_get_actionVal(msg); + memcpy( slugs_action, msg->payload, sizeof(mavlink_slugs_action_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index f1a54cb3e4..0c8d87f005 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -1,6 +1,8 @@ // MESSAGE SLUGS_NAVIGATION PACKING #define MAVLINK_MSG_ID_SLUGS_NAVIGATION 176 +#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN 30 +#define MAVLINK_MSG_176_LEN 30 typedef struct __mavlink_slugs_navigation_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_slugs_navigation_t } mavlink_slugs_navigation_t; - - /** * @brief Pack a slugs_navigation message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_slugs_navigation_t */ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - uint16_t i = 0; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - uint16_t i = 0; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui * @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_message_t msg; - mavlink_msg_slugs_navigation_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, u_m, phi_c, theta_c, psiDot_c, ay_body, totalDist, dist2Go, fromWP, toWP); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; + msg.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_slugs_navigation_t *p = &payload; + + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; + hdr.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,12 +195,8 @@ static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_slugs_navigation_get_u_m(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->u_m); } /** @@ -149,12 +206,8 @@ static inline float mavlink_msg_slugs_navigation_get_u_m(const mavlink_message_t */ static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->phi_c); } /** @@ -164,12 +217,8 @@ static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message */ static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->theta_c); } /** @@ -179,12 +228,8 @@ static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_messa */ static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->psiDot_c); } /** @@ -194,12 +239,8 @@ static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_mess */ static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->ay_body); } /** @@ -209,12 +250,8 @@ static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_messa */ static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->totalDist); } /** @@ -224,12 +261,8 @@ static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_mes */ static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->dist2Go); } /** @@ -239,7 +272,8 @@ static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_messa */ static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (uint8_t)(p->fromWP); } /** @@ -249,7 +283,8 @@ static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_mess */ static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (uint8_t)(p->toWP); } /** @@ -260,13 +295,5 @@ static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_messag */ static inline void mavlink_msg_slugs_navigation_decode(const mavlink_message_t* msg, mavlink_slugs_navigation_t* slugs_navigation) { - slugs_navigation->u_m = mavlink_msg_slugs_navigation_get_u_m(msg); - slugs_navigation->phi_c = mavlink_msg_slugs_navigation_get_phi_c(msg); - slugs_navigation->theta_c = mavlink_msg_slugs_navigation_get_theta_c(msg); - slugs_navigation->psiDot_c = mavlink_msg_slugs_navigation_get_psiDot_c(msg); - slugs_navigation->ay_body = mavlink_msg_slugs_navigation_get_ay_body(msg); - slugs_navigation->totalDist = mavlink_msg_slugs_navigation_get_totalDist(msg); - slugs_navigation->dist2Go = mavlink_msg_slugs_navigation_get_dist2Go(msg); - slugs_navigation->fromWP = mavlink_msg_slugs_navigation_get_fromWP(msg); - slugs_navigation->toWP = mavlink_msg_slugs_navigation_get_toWP(msg); + memcpy( slugs_navigation, msg->payload, sizeof(mavlink_slugs_navigation_t)); } diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index b8a6ff330a..e742be3584 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -43,13 +43,6 @@ extern "C" { #include "./mavlink_msg_mid_lvl_cmds.h" #include "./mavlink_msg_ctrl_srfc_pt.h" #include "./mavlink_msg_slugs_action.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 30b060f630..39a4c44a6b 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "ualberta.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index a011fc1ab4..d278887fe6 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -1,6 +1,8 @@ // MESSAGE NAV_FILTER_BIAS PACKING #define MAVLINK_MSG_ID_NAV_FILTER_BIAS 220 +#define MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN 32 +#define MAVLINK_MSG_220_LEN 32 typedef struct __mavlink_nav_filter_bias_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_nav_filter_bias_t } mavlink_nav_filter_bias_t; - - /** * @brief Pack a nav_filter_bias message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_nav_filter_bias_t */ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - uint16_t i = 0; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - uint16_t i = 0; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin * @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_message_t msg; - mavlink_msg_nav_filter_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, accel_0, accel_1, accel_2, gyro_0, gyro_1, gyro_2); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg.payload[0]; + + 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] + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; + msg.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_nav_filter_bias_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_nav_filter_bias_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_nav_filter_bias_get_usec(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_0(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_0); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_0(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_1); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_1(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_2); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_2(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_gyro_0(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_0); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_0(const mavlink_message */ static inline float mavlink_msg_nav_filter_bias_get_gyro_1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_1); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_1(const mavlink_message */ static inline float mavlink_msg_nav_filter_bias_get_gyro_2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_2); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_2(const mavlink_message */ static inline void mavlink_msg_nav_filter_bias_decode(const mavlink_message_t* msg, mavlink_nav_filter_bias_t* nav_filter_bias) { - nav_filter_bias->usec = mavlink_msg_nav_filter_bias_get_usec(msg); - nav_filter_bias->accel_0 = mavlink_msg_nav_filter_bias_get_accel_0(msg); - nav_filter_bias->accel_1 = mavlink_msg_nav_filter_bias_get_accel_1(msg); - nav_filter_bias->accel_2 = mavlink_msg_nav_filter_bias_get_accel_2(msg); - nav_filter_bias->gyro_0 = mavlink_msg_nav_filter_bias_get_gyro_0(msg); - nav_filter_bias->gyro_1 = mavlink_msg_nav_filter_bias_get_gyro_1(msg); - nav_filter_bias->gyro_2 = mavlink_msg_nav_filter_bias_get_gyro_2(msg); + memcpy( nav_filter_bias, msg->payload, sizeof(mavlink_nav_filter_bias_t)); } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 5907aba95b..0c5e7972ef 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -1,6 +1,8 @@ // MESSAGE RADIO_CALIBRATION PACKING #define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 +#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42 +#define MAVLINK_MSG_221_LEN 42 typedef struct __mavlink_radio_calibration_t { @@ -12,7 +14,6 @@ typedef struct __mavlink_radio_calibration_t uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) } mavlink_radio_calibration_t; - #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3 @@ -20,7 +21,6 @@ typedef struct __mavlink_radio_calibration_t #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 - /** * @brief Pack a radio_calibration message * @param system_id ID of this system @@ -37,17 +37,17 @@ typedef struct __mavlink_radio_calibration_t */ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - uint16_t i = 0; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } /** @@ -66,17 +66,17 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - uint16_t i = 0; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } /** @@ -104,12 +104,67 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param throttle Throttle curve setpoints (every 25%) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { mavlink_message_t msg; - mavlink_msg_radio_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, aileron, elevator, rudder, gyro, pitch, throttle); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg.payload[0]; + + 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%) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; + msg.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_radio_calibration_t *p = &payload; + + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; + hdr.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -120,11 +175,12 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co * * @return Aileron setpoints: left, center, right */ -static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* aileron) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(aileron, p->aileron, sizeof(p->aileron)); + return sizeof(p->aileron); } /** @@ -132,11 +188,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_m * * @return Elevator setpoints: nose down, center, nose up */ -static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* elevator) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(elevator, p->elevator, sizeof(p->elevator)); + return sizeof(p->elevator); } /** @@ -144,11 +201,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_ * * @return Rudder setpoints: nose left, center, nose right */ -static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* rudder) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(rudder, p->rudder, sizeof(p->rudder)); + return sizeof(p->rudder); } /** @@ -156,11 +214,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_me * * @return Tail gyro mode/gain setpoints: heading hold, rate mode */ -static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* gyro) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*2); - return sizeof(uint16_t)*2; + memcpy(gyro, p->gyro, sizeof(p->gyro)); + return sizeof(p->gyro); } /** @@ -168,11 +227,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_mess * * @return Pitch curve setpoints (every 25%) */ -static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* pitch) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; + memcpy(pitch, p->pitch, sizeof(p->pitch)); + return sizeof(p->pitch); } /** @@ -180,11 +240,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_mes * * @return Throttle curve setpoints (every 25%) */ -static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* throttle) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2+sizeof(uint16_t)*5, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; + memcpy(throttle, p->throttle, sizeof(p->throttle)); + return sizeof(p->throttle); } /** @@ -195,10 +256,5 @@ static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_ */ static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration) { - mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron); - mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator); - mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder); - mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro); - mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); - mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); + memcpy( radio_calibration, msg->payload, sizeof(mavlink_radio_calibration_t)); } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h deleted file mode 100644 index 64202ffe26..0000000000 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h +++ /dev/null @@ -1,101 +0,0 @@ -// MESSAGE REQUEST_RC_CHANNELS PACKING - -#define MAVLINK_MSG_ID_REQUEST_RC_CHANNELS 221 - -typedef struct __mavlink_request_rc_channels_t -{ - uint8_t enabled; ///< True: start sending data; False: stop sending data - -} mavlink_request_rc_channels_t; - - - -/** - * @brief Pack a request_rc_channels message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param enabled True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a request_rc_channels message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a request_rc_channels struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param request_rc_channels C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_rc_channels_t* request_rc_channels) -{ - return mavlink_msg_request_rc_channels_pack(system_id, component_id, msg, request_rc_channels->enabled); -} - -/** - * @brief Send a request_rc_channels message - * @param chan MAVLink channel to send the message - * - * @param enabled True: start sending data; False: stop sending data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_rc_channels_send(mavlink_channel_t chan, uint8_t enabled) -{ - mavlink_message_t msg; - mavlink_msg_request_rc_channels_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_RC_CHANNELS UNPACKING - -/** - * @brief Get field enabled from request_rc_channels message - * - * @return True: start sending data; False: stop sending data - */ -static inline uint8_t mavlink_msg_request_rc_channels_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Decode a request_rc_channels message into a struct - * - * @param msg The message to decode - * @param request_rc_channels C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_rc_channels_decode(const mavlink_message_t* msg, mavlink_request_rc_channels_t* request_rc_channels) -{ - request_rc_channels->enabled = mavlink_msg_request_rc_channels_get_enabled(msg); -} diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h index 50e8f7d02c..5546cdc5f3 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -1,6 +1,8 @@ // MESSAGE UALBERTA_SYS_STATUS PACKING #define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222 +#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN 3 +#define MAVLINK_MSG_222_LEN 3 typedef struct __mavlink_ualberta_sys_status_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_ualberta_sys_status_t } mavlink_ualberta_sys_status_t; - - /** * @brief Pack a ualberta_sys_status message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_ualberta_sys_status_t */ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - uint16_t i = 0; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - uint16_t i = 0; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, * @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_message_t msg; - mavlink_msg_ualberta_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, pilot); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg.payload[0]; + + 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 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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; + uint16_t checksum; + mavlink_ualberta_sys_status_t *p = &payload; + + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_mes */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -118,7 +169,8 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->pilot); } /** @@ -129,7 +181,5 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_me */ static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_t* msg, mavlink_ualberta_sys_status_t* ualberta_sys_status) { - ualberta_sys_status->mode = mavlink_msg_ualberta_sys_status_get_mode(msg); - ualberta_sys_status->nav_mode = mavlink_msg_ualberta_sys_status_get_nav_mode(msg); - ualberta_sys_status->pilot = mavlink_msg_ualberta_sys_status_get_pilot(msg); + memcpy( ualberta_sys_status, msg->payload, sizeof(mavlink_ualberta_sys_status_t)); } diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index b492ff62d6..b957f6d73a 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -30,7 +30,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Available autopilot modes for ualberta uav */ +/** @brief Available autopilot modes for ualberta uav */ enum UALBERTA_AUTOPILOT_MODE { MODE_MANUAL_DIRECT=0, /* */ @@ -41,7 +41,7 @@ enum UALBERTA_AUTOPILOT_MODE UALBERTA_AUTOPILOT_MODE_ENUM_END }; -/** @brief Navigation filter mode */ +/** @brief Navigation filter mode */ enum UALBERTA_NAV_MODE { NAV_AHRS_INIT=0, @@ -51,7 +51,7 @@ enum UALBERTA_NAV_MODE UALBERTA_NAV_MODE_ENUM_END }; -/** @brief Mode currently commanded by pilot */ +/** @brief Mode currently commanded by pilot */ enum UALBERTA_PILOT_MODE { PILOT_MANUAL=0, /* */ @@ -66,13 +66,6 @@ enum UALBERTA_PILOT_MODE #include "./mavlink_msg_nav_filter_bias.h" #include "./mavlink_msg_radio_calibration.h" #include "./mavlink_msg_ualberta_sys_status.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 5df9ba827e..de21876a88 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -1,892 +1,1131 @@ - + -2 - - - - Commands to be executed by the MAV. They can be executed on user request, + 3 + + + Micro air vehicle / autopilot classes. This identifies the individual model. + + Generic autopilot, full support for everything + + + PIXHAWK autopilot, http://pixhawk.ethz.ch + + + SLUGS autopilot, http://slugsuav.soe.ucsc.edu + + + ArduPilotMega / ArduCopter, http://diydrones.com + + + OpenPilot, http://openpilot.org + + + Generic autopilot only supporting simple waypoints + + + Generic autopilot supporting waypoints and other simple navigation commands + + + Generic autopilot supporting the full mission command set + + + No valid autopilot, e.g. a GCS or other MAVLink component + + + PPZ UAV - http://nongnu.org/paparazzi + + + UAV Dev Board + + + FlexiPilot + + + + + + System is in undefined state + + + Motors are blocked, system is safe + + + System is allowed to be active, under manual (RC) control + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control and navigation + + + Generic test mode, for custom use + + + Generic test mode, for custom use + + + Generic test mode, for custom use + + + System is ready, motors are unblocked, but controllers are inactive + + + System is blocked, only RC valued are read and reported back + + + + + System is currently on ground. + + + System is during liftoff, not in normal navigation mode yet. + + + System is holding its current position (rotorcraft or rover / boat). + + + System is navigating towards the next waypoint. + + + System is flying at a defined course and speed. + + + System is return straight to home position. + + + System is landing. + + + System lost its position input and is in attitude / flight stabilization only. + + + System is loitering in wait position. DO NOT USE THIS FOR WAYPOINT LOITER! + + + + + Uninitialized system, state is unknown. + + + System is booting up. + + + System is calibrating and not flight-ready. + + + System is grounded and on standby. It can be launched any time. + + + System is active and might be already airborne. Motors are engaged. + + + System is in a non-normal flight mode. It can however still navigate. + + + System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. + + + System just initialized its power-down sequence, will shut down now. + + + + Data stream IDs. A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. + + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + + + The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). + + + Point toward next waypoint. + + + Point toward given waypoint. + + + Point toward fixed location. + + + Point toward of given id. + + + + + Generic micro air vehicle. + + + Fixed wing aircraft. + + + Quadrotor + + + Coaxial helicopter + + + Normal helicopter with tail rotor. + + + Ground installation + + + Operator control unit / ground control station + + + Airship, controlled + + + Free balloon, uncontrolled + + + Rocket + + + Ground rover + + + Surface vessel, boat, ship + + + Submarine + + + Hexarotor + + + Octorotor + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Global coordinate frame, WGS84 coordinate system. + + + Local coordinate frame, Z-up (x: north, y: east, z: down). + + + NOT a coordinate frame, indicates a mission command. + + + Global coordinate frame, WGS84 coordinate system, relative altitude over ground. + + + Local coordinate frame, Z-down (x: east, y: north, z: up) + + + + + + + + + + + + + + + + + + + + + + + + Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. - - Navigate to waypoint. - Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) - Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) - 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. - Desired yaw angle at waypoint (rotary wing) - Latitude - Longitude - Altitude - - - Loiter around this waypoint an unlimited amount of time - Empty - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Loiter around this waypoint for X turns - Turns - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Loiter around this waypoint for X seconds - Seconds (decimal) - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Return to launch location - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Land at location - Empty - Empty - Empty - Desired yaw angle. - Latitude - Longitude - Altitude - - - Takeoff from ground / hand - Minimum pitch (if airspeed sensor present), desired pitch without sensor - Empty - Empty - Yaw angle (if magnetometer present), ignored without magnetometer - Latitude - Longitude - Altitude - - - - Sets the region of interest (ROI) for a sensor set or the + + Navigate to waypoint. + Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing) + Latitude + Longitude + Altitude + + + Loiter around this waypoint an unlimited amount of time + Empty + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X turns + Turns + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X seconds + Seconds (decimal) + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location + Empty + Empty + Empty + Desired yaw angle. + Latitude + Longitude + Altitude + + + Takeoff from ground / hand + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer + Latitude + Longitude + Altitude + + + Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. - Region of intereset mode. (see MAV_ROI enum) - Waypoint index/ target ID. (see MAV_ROI enum) - ROI index (allows a vehicle to manage multiple ROI's) - Empty - x the location of the fixed ROI (see MAV_FRAME) - y - z - - - - Control autonomous path planning on the MAV. - 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning - 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid - Empty - Yaw angle at goal, in compass degrees, [0..360] - Latitude/X of goal - Longitude/Y of goal - Altitude/Z of goal - - - NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Delay mission state machine. - Delay in seconds (decimal) - Empty - Empty - Empty - Empty - Empty - Empty - - - Ascend/descend at rate. Delay mission state machine until desired altitude reached. - Descent / Ascend rate (m/s) - Empty - Empty - Empty - Empty - Empty - Finish Altitude - - - Delay mission state machine until within desired distance of next NAV point. - Distance (meters) - Empty - Empty - Empty - Empty - Empty - Empty - - - Reach a certain target angle. - target angle: [0-360], 0 is north - speed during yaw change:[deg per second] - direction: negative: counter clockwise, positive: clockwise [-1,1] - relative offset or absolute angle: [ 1,0] - Empty - Empty - Empty - - - NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Set system mode. - Mode, as defined by ENUM MAV_MODE - Empty - Empty - Empty - Empty - Empty - Empty - - - Jump to the desired command in the mission list. Repeat this action only the specified number of times - Sequence number - Repeat count - Empty - Empty - Empty - Empty - Empty - - - Change speed and/or throttle set points. - Speed type (0=Airspeed, 1=Ground Speed) - Speed (m/s, -1 indicates no change) - Throttle ( Percent, -1 indicates no change) - Empty - Empty - Empty - Empty - - - Changes the home location either to the current location or a specified location. - Use current (1=use current location, 0=use specified location) - Empty - Empty - Empty - Latitude - Longitude - Altitude - - - Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. - Parameter number - Parameter value - Empty - Empty - Empty - Empty - Empty - - - Set a relay to a condition. - Relay number - Setting (1=on, 0=off, others possible depending on system hardware) - Empty - Empty - Empty - Empty - Empty - - - Cycle a relay on and off for a desired number of cyles with a desired period. - Relay number - Cycle count - Cycle time (seconds, decimal) - Empty - Empty - Empty - Empty - - - Set a servo to a desired PWM value. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Empty - Empty - Empty - Empty - Empty - - - Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Cycle count - Cycle time (seconds) - Empty - Empty - Empty - - - Control onboard camera system. - Camera ID (-1 for all) - Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw - Transmission mode: 0: video stream, >0: single images every n seconds (decimal) - Recording: 0: disabled, 1: enabled compressed, 2: enabled raw - Empty - Empty - Empty - - - NOP - This command is only used to mark the upper limit of the DO commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Trigger calibration. This command will be only accepted if in pre-flight mode. - Gyro calibration: 0: no, 1: yes - Magnetometer calibration: 0: no, 1: yes - Ground pressure: 0: no, 1: yes - Radio calibration: 0: no, 1: yes - Empty - Empty - Empty - - - Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. - Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM - Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM - Reserved - Reserved - Empty - Empty - Empty - - - - Data stream IDs. A data stream is not a fixed set of messages, but rather a - recommendation to the autopilot software. Individual autopilots may or may not obey - the recommended messages. - - Enable all data streams - Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. - Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS - Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW - Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. - Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. - Dependent on the autopilot - Dependent on the autopilot - Dependent on the autopilot - - - - The ROI (region of interest) for the vehicle. This can be - be used by the vehicle for camera/vehicle attitude alignment (see - MAV_CMD_NAV_ROI). - - Point toward next waypoint. - Point toward given waypoint. - Point toward fixed location. - Point toward of given id. - - - - - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - - - The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. - The onboard software version - - - - The system time is the time of the master clock, typically the computer clock of the main onboard computer. - Timestamp of the master clock in microseconds since UNIX epoch. - - - - A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. - PING sequence - 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - Unix timestamp in microseconds - - - - UTC date and time from GPS module - GPS UTC date ddmmyy - GPS UTC time hhmmss - - - - Request to control this MAV - System the GCS requests control for - 0: request control of this MAV, 1: Release control of this MAV - 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - - - - Accept / deny control of this MAV - ID of the GCS this message - 0: request control of this MAV, 1: Release control of this MAV - 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - - - Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. - key - - - - This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The action id - 0: Action DENIED, 1: Action executed - - - - An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The system executing the action - The component executing the action - The action id - - - - Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. - The system setting the mode - The new mode - - - - Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. - The system setting the mode - The new navigation mode - - - - Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. - System ID - Component ID - Onboard parameter id - Parameter index. Send -1 to use the param ID field as identifier - - - - Request all parameters of this component. After his request, all parameters are emitted. - System ID - Component ID - - - - Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id - Onboard parameter value - Total number of onboard parameters - Index of this onboard parameter - - - - Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. - System ID - Component ID - Onboard parameter id - Onboard parameter value - - - - The global position, as returned by the Global Positioning System (GPS). This is + Region of intereset mode. (see MAV_ROI enum) + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + Control autonomous path planning on the MAV. + 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning + 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid + Empty + Yaw angle at goal, in compass degrees, [0..360] + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay mission state machine. + Delay in seconds (decimal) + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend at rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate (m/s) + Empty + Empty + Empty + Empty + Empty + Finish Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance (meters) + Empty + Empty + Empty + Empty + Empty + Empty + + + Reach a certain target angle. + target angle: [0-360], 0 is north + speed during yaw change:[deg per second] + direction: negative: counter clockwise, positive: clockwise [-1,1] + relative offset or absolute angle: [ 1,0] + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Set system mode. + Mode, as defined by ENUM MAV_MODE + Empty + Empty + Empty + Empty + Empty + Empty + + + Jump to the desired command in the mission list. Repeat this action only the specified number of times + Sequence number + Repeat count + Empty + Empty + Empty + Empty + Empty + + + Change speed and/or throttle set points. + Speed type (0=Airspeed, 1=Ground Speed) + Speed (m/s, -1 indicates no change) + Throttle ( Percent, -1 indicates no change) + Empty + Empty + Empty + Empty + + + Changes the home location either to the current location or a specified location. + Use current (1=use current location, 0=use specified location) + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. + Parameter number + Parameter value + Empty + Empty + Empty + Empty + Empty + + + Set a relay to a condition. + Relay number + Setting (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + Cycle a relay on and off for a desired number of cyles with a desired period. + Relay number + Cycle count + Cycle time (seconds, decimal) + Empty + Empty + Empty + Empty + + + Set a servo to a desired PWM value. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Empty + Empty + Empty + Empty + Empty + + + Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Cycle count + Cycle time (seconds) + Empty + Empty + Empty + + + Control onboard camera system. + Camera ID (-1 for all) + Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw + Transmission mode: 0: video stream, >0: single images every n seconds (decimal) + Recording: 0: disabled, 1: enabled compressed, 2: enabled raw + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the DO commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Trigger calibration. This command will be only accepted if in pre-flight mode. + Gyro calibration: 0: no, 1: yes + Magnetometer calibration: 0: no, 1: yes + Ground pressure: 0: no, 1: yes + Radio calibration: 0: no, 1: yes + Empty + Empty + Empty + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Reserved + Reserved + Empty + Empty + Empty + + + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + + The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. + The onboard software version + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp of the master clock in microseconds since UNIX epoch. + + + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. + PING sequence + 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + Unix timestamp in microseconds + + + UTC date and time from GPS module + GPS UTC date ddmmyy + GPS UTC time hhmmss + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The action id + 0: Action DENIED, 1: Action executed + + + An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The system executing the action + The component executing the action + The action id + + + Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new mode + + + Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. + The system setting the mode + The new navigation mode + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id + Parameter index. Send -1 to use the param ID field as identifier + + + Request all parameters of this component. After his request, all parameters are emitted. + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. + Onboard parameter id + Onboard parameter value + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. + System ID + Component ID + Onboard parameter id + Onboard parameter value + + + The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - Latitude in 1E7 degrees - Longitude in 1E7 degrees - Altitude in 1E3 meters (millimeters) - GPS HDOP - GPS VDOP - GPS ground speed (m/s) - Compass heading in degrees, 0..360 degrees - - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (mg) - Y acceleration (mg) - Z acceleration (mg) - Angular speed around X axis (millirad /sec) - Angular speed around Y axis (millirad /sec) - Angular speed around Z axis (millirad /sec) - X Magnetic field (milli tesla) - Y Magnetic field (milli tesla) - Z Magnetic field (milli tesla) - - - - The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. - Number of satellites visible - Global satellite ID - 0: Satellite not used, 1: used for localization - Elevation (0: right on top of receiver, 90: on the horizon) of satellite - Direction of satellite, 0: 0 deg, 255: 360 deg. - Signal to noise ratio of satellite - - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (raw) - Y acceleration (raw) - Z acceleration (raw) - Angular speed around X axis (raw) - Angular speed around Y axis (raw) - Angular speed around Z axis (raw) - X Magnetic field (raw) - Y Magnetic field (raw) - Z Magnetic field (raw) - - - - The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (raw) - Differential pressure 1 (raw) - Differential pressure 2 (raw) - Raw Temperature measurement (raw) - - - - The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (hectopascal) - Differential pressure 1 (hectopascal) - Temperature measurement (0.01 degrees celsius) - - - - - The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Roll angle (rad) - Pitch angle (rad) - Yaw angle (rad) - Roll angular speed (rad/s) - Pitch angular speed (rad/s) - Yaw angular speed (rad/s) - - - - The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X Position - Y Position - Z Position - X Speed - Y Speed - Z Speed - - - - - The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since unix epoch) - Latitude, in degrees - Longitude, in degrees - Absolute altitude, in meters - X Speed (in Latitude direction, positive: going north) - Y Speed (in Longitude direction, positive: going east) - Z Speed (in Altitude direction, positive: going up) - - - - The global position, as returned by the Global Positioning System (GPS). This is + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in 1E7 degrees + Longitude in 1E7 degrees + Altitude in 1E3 meters (millimeters) + GPS HDOP + GPS VDOP + GPS ground speed (m/s) + Compass heading in degrees, 0..360 degrees + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + Angular speed around X axis (millirad /sec) + Angular speed around Y axis (millirad /sec) + Angular speed around Z axis (millirad /sec) + X Magnetic field (milli tesla) + Y Magnetic field (milli tesla) + Z Magnetic field (milli tesla) + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (raw) + Differential pressure 1 (raw) + Differential pressure 2 (raw) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (hectopascal) + Differential pressure 1 (hectopascal) + Temperature measurement (0.01 degrees celsius) + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since unix epoch) + Latitude, in degrees + Longitude, in degrees + Absolute altitude, in meters + X Speed (in Latitude direction, positive: going north) + Y Speed (in Longitude direction, positive: going east) + Z Speed (in Altitude direction, positive: going up) + + + The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - Latitude in degrees - Longitude in degrees - Altitude in meters - GPS HDOP - GPS VDOP - GPS ground speed - Compass heading in degrees, 0..360 degrees - - - - The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. - System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - Navigation mode, see MAV_NAV_MODE ENUM - System status flag, see MAV_STATUS ENUM - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Battery voltage, in millivolts (1 = 1 millivolt) - Remaining battery energy: (0%: 0, 100%: 1000) - Dropped packets (packets that were corrupted on reception on the MAV) - - - - The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - RC channel 1 value, in microseconds - RC channel 2 value, in microseconds - RC channel 3 value, in microseconds - RC channel 4 value, in microseconds - RC channel 5 value, in microseconds - RC channel 6 value, in microseconds - RC channel 7 value, in microseconds - RC channel 8 value, in microseconds - Receive signal strength indicator, 0: 0%, 255: 100% - - - - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - Receive signal strength indicator, 0: 0%, 255: 100% - - - - The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. - Servo output 1 value, in microseconds - Servo output 2 value, in microseconds - Servo output 3 value, in microseconds - Servo output 4 value, in microseconds - Servo output 5 value, in microseconds - Servo output 6 value, in microseconds - Servo output 7 value, in microseconds - Servo output 8 value, in microseconds - - - - Message encoding a waypoint. This message is emitted to announce + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in degrees + Longitude in degrees + Altitude in meters + GPS HDOP + GPS VDOP + GPS ground speed + Compass heading in degrees, 0..360 degrees + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + Navigation mode, see MAV_NAV_MODE ENUM + System status flag, see MAV_STATUS ENUM + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Battery voltage, in millivolts (1 = 1 millivolt) + Remaining battery energy: (0%: 0, 100%: 1000) + Dropped packets (packets that were corrupted on reception on the MAV) + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + + The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + Receive signal strength indicator, 0: 0%, 255: 100% + + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Servo output 1 value, in microseconds + Servo output 2 value, in microseconds + Servo output 3 value, in microseconds + Servo output 4 value, in microseconds + Servo output 5 value, in microseconds + Servo output 6 value, in microseconds + Servo output 7 value, in microseconds + Servo output 8 value, in microseconds + + + Message encoding a waypoint. This message is emitted to announce the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed - System ID - Component ID - Sequence - The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - false:0, true:1 - autocontinue to next wp - PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - PARAM5 / local: x position, global: latitude - PARAM6 / y position: global: longitude - PARAM7 / z position: global: altitude - - - - Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. - System ID - Component ID - Sequence - - - - Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). - System ID - Component ID - Sequence - - - - Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. - Sequence - - - - Request the overall list of waypoints from the system/component. - System ID - Component ID - - - - This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. - System ID - Component ID - Number of Waypoints in the Sequence - - - - Delete all waypoints at once. - System ID - Component ID - - - - A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. - Sequence - - - - Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). - System ID - Component ID - 0: OK, 1: Error - - - - As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. - System ID - Component ID - global position * 1E7 - global position * 1E7 - global position * 1000 - - - - Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position - Latitude (WGS84), expressed as * 1E7 - Longitude (WGS84), expressed as * 1E7 - Altitude(WGS84), expressed as * 1000 - - - - Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. - System ID - Component ID - x position - y position - z position - Desired yaw angle - - - - Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. - x position - y position - z position - Desired yaw angle - - - - Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - Attitude estimation health: 0: poor, 255: excellent - 0: Attitude control disabled, 1: enabled - 0: X, Y position control disabled, 1: enabled - 0: Z position control disabled, 1: enabled - 0: Yaw angle control disabled, 1: enabled - - - - Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. - System ID - Component ID - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - - Read out the safety zone the MAV currently assumes. - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - - The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Attitude roll: -128: -100%, 127: +100% - Attitude pitch: -128: -100%, 127: +100% - Attitude yaw: -128: -100%, 127: +100% - Attitude thrust: -128: -100%, 127: +100% - - - - The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Position x: -128: -100%, 127: +100% - Position y: -128: -100%, 127: +100% - Position z: -128: -100%, 127: +100% - Position yaw: -128: -100%, 127: +100% - - - - Outputs of the APM navigation controller. The primary use of this message is to check the response and signs + System ID + Component ID + Sequence + The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + false:0, true:1 + autocontinue to next wp + PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + PARAM5 / local: x position, global: latitude + PARAM6 / y position: global: longitude + PARAM7 / z position: global: altitude + + + Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. + System ID + Component ID + Sequence + + + Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. + Sequence + + + Request the overall list of waypoints from the system/component. + System ID + Component ID + + + This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of Waypoints in the Sequence + + + Delete all waypoints at once. + System ID + Component ID + + + A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + 0: OK, 1: Error + + + As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Component ID + global position * 1E7 + global position * 1E7 + global position * 1000 + + + Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position + Latitude (WGS84), expressed as * 1E7 + Longitude (WGS84), expressed as * 1E7 + Altitude(WGS84), expressed as * 1000 + + + Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. + System ID + Component ID + x position + y position + z position + Desired yaw angle + + + Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. + x position + y position + z position + Desired yaw angle + + + Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + Attitude estimation health: 0: poor, 255: excellent + 0: Attitude control disabled, 1: enabled + 0: X, Y position control disabled, 1: enabled + 0: Z position control disabled, 1: enabled + 0: Yaw angle control disabled, 1: enabled + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. + 1: enabled, 0: disabled + Attitude roll: -128: -100%, 127: +100% + Attitude pitch: -128: -100%, 127: +100% + Attitude yaw: -128: -100%, 127: +100% + Attitude thrust: -128: -100%, 127: +100% + + + The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. + 1: enabled, 0: disabled + Position x: -128: -100%, 127: +100% + Position y: -128: -100%, 127: +100% + Position z: -128: -100%, 127: +100% + Position yaw: -128: -100%, 127: +100% + + + Outputs of the APM navigation controller. The primary use of this message is to check the response and signs of the controller before actual flight and to assist with tuning controller parameters - Current desired roll in degrees - Current desired pitch in degrees - Current desired heading in degrees - Bearing to current waypoint/target in degrees - Distance to active waypoint in meters - Current altitude error in meters - Current airspeed error in meters/second - Current crosstrack error on x-y plane in meters - - - - The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. - x position error - y position error - z position error - roll error (radians) - pitch error (radians) - yaw error (radians) - x velocity - y velocity - z velocity - - - - The system setting the altitude - The new altitude in meters - - - - The target requested to send the message stream. - The target requested to send the message stream. - The ID of the requested message type - The requested interval between two messages of this type - 1 to start sending, 0 to stop sending. - - - - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - - The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) - Latitude, expressed as * 1E7 - Longitude, expressed as * 1E7 - Altitude in meters, expressed as * 1000 (millimeters) - Ground X Speed (Latitude), expressed as m/s * 100 - Ground Y Speed (Longitude), expressed as m/s * 100 - Ground Z Speed (Altitude), expressed as m/s * 100 - - - - Metrics typically displayed on a HUD for fixed wing aircraft - Current airspeed in m/s - Current ground speed in m/s - Current heading in degrees, in compass units (0..360, 0=north) - Current throttle setting in integer percent, 0 to 100 - Current altitude (MSL), in meters - Current climb rate in meters/second - - - - Send a command with up to four parameters to the MAV - System which should execute the command - Component which should execute the command, 0 for all components - Command ID, as defined by MAV_CMD enum. - 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - Parameter 1, as defined by MAV_CMD enum. - Parameter 2, as defined by MAV_CMD enum. - Parameter 3, as defined by MAV_CMD enum. - Parameter 4, as defined by MAV_CMD enum. - - - - Report status of a command. Includes feedback wether the command was executed - Current airspeed in m/s - 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - - - - - - - - - Name - Timestamp - x - y - z - - - - Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Floating point value - - - - Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Signed integer value - - - - Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). - Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character - - - - Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. - index of debug variable - DEBUG value - - - + Current desired roll in degrees + Current desired pitch in degrees + Current desired heading in degrees + Bearing to current waypoint/target in degrees + Distance to active waypoint in meters + Current altitude error in meters + Current airspeed error in meters/second + Current crosstrack error on x-y plane in meters + + + The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. + x position error + y position error + z position error + roll error (radians) + pitch error (radians) + yaw error (radians) + x velocity + y velocity + z velocity + + + The system setting the altitude + The new altitude in meters + + + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested message type + The requested interval between two messages of this type + 1 to start sending, 0 to stop sending. + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + + + Metrics typically displayed on a HUD for fixed wing aircraft + Current airspeed in m/s + Current ground speed in m/s + Current heading in degrees, in compass units (0..360, 0=north) + Current throttle setting in integer percent, 0 to 100 + Current altitude (MSL), in meters + Current climb rate in meters/second + + + Send a command with up to four parameters to the MAV + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + + + Report status of a command. Includes feedback wether the command was executed + Current airspeed in m/s + 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + + + Name + Timestamp + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status, 0 = info message, 255 = critical fault + Status text message, without null termination character + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + index of debug variable + DEBUG value + + diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index e2a99a738b..46f18b1544 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -175,8 +175,8 @@ Watchdog ID Process ID - Process name - Process arguments + Process name + Process arguments Timeout (seconds) @@ -199,7 +199,7 @@ 0: Pattern, 1: Letter Confidence of detection - Pattern file name + Pattern file name Accepted as true detection, 0 no, 1 yes @@ -214,7 +214,7 @@ X Position Y Position Z Position - POI name + POI name @@ -231,7 +231,7 @@ X2 Position Y2 Position Z2 Position - POI connection name + POI connection name @@ -254,7 +254,7 @@ Orientation assignment 0: false, 1:true Size in pixels Orientation - Descriptor + Descriptor Harris operator response at this location diff --git a/thirdParty/mavlink/missionlib/testing/.gitignore b/thirdParty/mavlink/missionlib/testing/.gitignore new file mode 100644 index 0000000000..4b4c3f6c4e --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/.gitignore @@ -0,0 +1,2 @@ +*.o +udptest diff --git a/thirdParty/mavlink/missionlib/testing/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c new file mode 100644 index 0000000000..37a8a7848e --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/udp.c @@ -0,0 +1,213 @@ +/******************************************************************************* + Copyright (C) 2010 Bryan Godbolt godbolt ( a t ) ualberta.ca + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ +/* + This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets + cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from + qgroundcontrol are printed by this program along with the heartbeats. + + + I compiled this program sucessfully on Ubuntu 10.04 with the following command + + gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c + + the rt library is needed for the clock_gettime on linux + */ +/* These headers are for QNX, but should all be standard on unix/linux */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if (defined __QNX__) | (defined __QNXNTO__) +/* QNX specific headers */ +#include +#else +/* Linux / MacOS POSIX timer headers */ +#include +#include +#include +#endif + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +uint64_t microsSinceEpoch(); + +int main(int argc, char* argv[]) +{ + + char help[] = "--help"; + + + char target_ip[100]; + + float position[6] = {}; + int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + struct sockaddr_in gcAddr; + struct sockaddr_in locAddr; + //struct sockaddr_in fromAddr; + uint8_t buf[BUFFER_LENGTH]; + ssize_t recsize; + socklen_t fromlen; + int bytes_sent; + mavlink_message_t msg; + uint16_t len; + int i = 0; + //int success = 0; + unsigned int temp = 0; + + // Check if --help flag was used + if ((argc == 2) && (strcmp(argv[1], help) == 0)) + { + printf("\n"); + printf("\tUsage:\n\n"); + printf("\t"); + printf("%s", argv[0]); + printf(" \n"); + printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); + exit(EXIT_FAILURE); + } + + + // Change the target ip if parameter was given + strcpy(target_ip, "127.0.0.1"); + if (argc == 2) + { + strcpy(target_ip, argv[1]); + } + + + memset(&locAddr, 0, sizeof(locAddr)); + locAddr.sin_family = AF_INET; + locAddr.sin_addr.s_addr = INADDR_ANY; + locAddr.sin_port = htons(14551); + + /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ + if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) + { + perror("error bind failed"); + close(sock); + exit(EXIT_FAILURE); + } + + /* Attempt to make it non blocking */ + if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); + close(sock); + exit(EXIT_FAILURE); + } + + + memset(&gcAddr, 0, sizeof(gcAddr)); + gcAddr.sin_family = AF_INET; + gcAddr.sin_addr.s_addr = inet_addr(target_ip); + gcAddr.sin_port = htons(14550); + + + + for (;;) + { + + /*Send Heartbeat */ + mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send Status */ + mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + /* Send Local Position */ + mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), + position[0], position[1], position[2], + position[3], position[4], position[5]); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send attitude */ + mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + + memset(buf, 0, BUFFER_LENGTH); + recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); + if (recsize > 0) + { + // Something received - print out all bytes and parse packet + mavlink_message_t msg; + mavlink_status_t status; + + printf("Bytes Received: %d\nDatagram: ", (int)recsize); + for (i = 0; i < recsize; ++i) + { + temp = buf[i]; + printf("%02x ", (unsigned char)temp); + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // Packet received + printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + sleep(1); // Sleep one second + } +} + + +/* QNX timer version */ +#if (defined __QNX__) | (defined __QNXNTO__) +uint64_t microsSinceEpoch() +{ + + struct timespec time; + + uint64_t micros = 0; + + clock_gettime(CLOCK_REALTIME, &time); + micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; + + return micros; +} +#else +uint64_t microsSinceEpoch() +{ + + struct timeval tv; + + uint64_t micros = 0; + + gettimeofday(&tv, NULL); + micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + + return micros; +} +#endif \ No newline at end of file -- GitLab From 52c34e850dbe9dd42bbae3551f8f9d9fad951fc5 Mon Sep 17 00:00:00 2001 From: lm Date: Sun, 31 Jul 2011 17:51:36 +0200 Subject: [PATCH 007/131] Updated to v1.0 --- src/comm/MAVLinkSimulationLink.cc | 65 ++-- src/comm/MAVLinkSimulationMAV.cc | 51 +-- src/comm/MAVLinkSimulationMAV.h | 17 +- src/uas/QGCMAVLinkUASFactory.cc | 25 +- src/uas/UAS.cc | 328 ++++++++-------- src/uas/UAS.h | 2 - src/uas/UASInterface.h | 2 - src/uas/UASWaypointManager.cc | 15 +- src/ui/DebugConsole.cc | 2 +- thirdParty/mavlink/include/protocol.h | 521 -------------------------- 10 files changed, 263 insertions(+), 765 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 50e87e9ff7..4ffd5ec4b1 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -546,14 +546,14 @@ void MAVLinkSimulationLink::mainloop() static int typeCounter = 0; uint8_t mavType; if (typeCounter < 10) { - mavType = MAV_QUADROTOR; + mavType = MAV_TYPE_QUADROTOR; } else { - mavType = typeCounter % (OCU); + mavType = typeCounter % (MAV_TYPE_OCU); } typeCounter++; // Pack message and get size of encoded byte string - messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK); + messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_CLASS_PIXHAWK); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -722,31 +722,32 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) qDebug() << "SIM" << "received action" << action.action << "for system" << action.target; - switch (action.action) { - case MAV_ACTION_LAUNCH: - status.status = MAV_STATE_ACTIVE; - status.mode = MAV_MODE_AUTO; - break; - case MAV_ACTION_RETURN: - status.status = MAV_STATE_ACTIVE; - break; - case MAV_ACTION_MOTORS_START: - status.status = MAV_STATE_ACTIVE; - status.mode = MAV_MODE_LOCKED; - break; - case MAV_ACTION_MOTORS_STOP: - status.status = MAV_STATE_STANDBY; - status.mode = MAV_MODE_LOCKED; - break; - case MAV_ACTION_EMCY_KILL: - status.status = MAV_STATE_EMERGENCY; - status.mode = MAV_MODE_MANUAL; - break; - case MAV_ACTION_SHUTDOWN: - status.status = MAV_STATE_POWEROFF; - status.mode = MAV_MODE_LOCKED; - break; - } + // FIXME MAVLINKV10PORTINGNEEDED +// switch (action.action) { +// case MAV_ACTION_LAUNCH: +// status.status = MAV_STATE_ACTIVE; +// status.mode = MAV_MODE_AUTO; +// break; +// case MAV_ACTION_RETURN: +// status.status = MAV_STATE_ACTIVE; +// break; +// case MAV_ACTION_MOTORS_START: +// status.status = MAV_STATE_ACTIVE; +// status.mode = MAV_MODE_LOCKED; +// break; +// case MAV_ACTION_MOTORS_STOP: +// status.status = MAV_STATE_STANDBY; +// status.mode = MAV_MODE_LOCKED; +// break; +// case MAV_ACTION_EMCY_KILL: +// status.status = MAV_STATE_EMERGENCY; +// status.mode = MAV_MODE_MANUAL; +// break; +// case MAV_ACTION_SHUTDOWN: +// status.status = MAV_STATE_POWEROFF; +// status.mode = MAV_MODE_LOCKED; +// break; +// } } break; #ifdef MAVLINK_ENABLED_PIXHAWK @@ -771,7 +772,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { if (j != 5) { // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), onboardParams.size(), j); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -799,7 +800,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) onboardParams.insert(key, set.param_value); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(set.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -820,7 +821,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -832,7 +833,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 356104601d..51205297a5 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -68,7 +68,7 @@ void MAVLinkSimulationMAV::mainloop() // 1 Hz execution if (timer1Hz <= 0) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version); + mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_CLASS_PIXHAWK); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); @@ -306,30 +306,31 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) if (systemid == mode.target) sys_mode = mode.mode; } break; - case MAVLINK_MSG_ID_ACTION: { - mavlink_action_t action; - mavlink_msg_action_decode(&msg, &action); - if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { - mavlink_action_ack_t ack; - ack.action = action.action; - switch (action.action) { - case MAV_ACTION_TAKEOFF: - flying = true; - nav_mode = MAV_NAV_LIFTOFF; - ack.result = 1; - break; - default: { - ack.result = 0; - } - break; - } - - // Give feedback about action - mavlink_message_t r_msg; - mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); - link->sendMAVLinkMessage(&r_msg); - } - } + // FIXME MAVLINKV10PORTINGNEEDED +// case MAVLINK_MSG_ID_ACTION: { +// mavlink_action_t action; +// mavlink_msg_action_decode(&msg, &action); +// if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { +// mavlink_action_ack_t ack; +// ack.action = action.action; +//// switch (action.action) { +//// case MAV_ACTION_TAKEOFF: +//// flying = true; +//// nav_mode = MAV_NAV_LIFTOFF; +//// ack.result = 1; +//// break; +//// default: { +//// ack.result = 0; +//// } +//// break; +//// } + +// // Give feedback about action +// mavlink_message_t r_msg; +// mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); +// link->sendMAVLinkMessage(&r_msg); +// } +// } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: { mavlink_local_position_setpoint_set_t sp; diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h index 4f24eb7c52..8417ea3508 100644 --- a/src/comm/MAVLinkSimulationMAV.h +++ b/src/comm/MAVLinkSimulationMAV.h @@ -55,16 +55,17 @@ protected: bool flying; int mavlink_version; - static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) { - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + // FIXME MAVLINKV10PORTINGNEEDED +// static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) { +// uint16_t i = 0; +// msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version +// i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) +// i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM +// i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version - return mavlink_finalize_message(msg, system_id, component_id, i); - } +// return mavlink_finalize_message(msg, system_id, component_id, i); +// } }; diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index e9081d960e..7b9703d2ac 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -10,16 +10,21 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte { QPointer p; - if (parent != NULL) { + if (parent != NULL) + { p = parent; - } else { + } + else + { p = mavlink; } UASInterface* uas; - switch (heartbeat->autopilot) { - case MAV_AUTOPILOT_GENERIC: { + switch (heartbeat->autopilot) + { + case MAV_CLASS_GENERIC: + { UAS* mav = new UAS(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -28,7 +33,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - case MAV_AUTOPILOT_PIXHAWK: { + case MAV_CLASS_PIXHAWK: + { PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -40,7 +46,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - case MAV_AUTOPILOT_SLUGS: { + case MAV_CLASS_SLUGS: + { SlugsMAV* mav = new SlugsMAV(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -52,7 +59,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - case MAV_AUTOPILOT_ARDUPILOTMEGA: { + case MAV_CLASS_ARDUPILOTMEGA: + { ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -64,7 +72,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - default: { + default: + { UAS* mav = new UAS(mavlink, sysid); mav->setSystemType((int)heartbeat->type); // Connect this robot to the UAS object diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1270ff149c..0349c2f831 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -205,10 +205,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { switch (type) { - case MAV_FIXED_WING: + case MAV_TYPE_FIXED_WING: setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); break; - case MAV_QUADROTOR: + case MAV_TYPE_QUADROTOR: setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); break; default: @@ -930,7 +930,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { QByteArray b; b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); - mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); + mavlink_msg_statustext_get_text(&message, b.data()); //b.append('\0'); QString text = QString(b); int severity = mavlink_msg_statustext_get_severity(&message); @@ -1163,14 +1163,15 @@ void UAS::setLocalOriginAtCurrentGPSPosition() if (ret == QMessageBox::Yes) { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN); - // Send message twice to increase chance that it reaches its goal - sendMessage(msg); - // Wait 5 ms - MG::SLEEP::usleep(5000); - // Send again - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN); +// // Send message twice to increase chance that it reaches its goal +// sendMessage(msg); +// // Wait 5 ms +// MG::SLEEP::usleep(5000); +// // Send again +// sendMessage(msg); result = true; } } @@ -1204,52 +1205,58 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) } void UAS::startRadioControlCalibration() -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC); - sendMessage(msg); +{// FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_DO_RC_CALIB; +// sendMessage(msg); } void UAS::startDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); +// sendMessage(msg); } void UAS::pauseDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); +// sendMessage(msg); } void UAS::stopDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); +// sendMessage(msg); } void UAS::startMagnetometerCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); +// sendMessage(msg); } void UAS::startGyroscopeCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); +// sendMessage(msg); } void UAS::startPressureCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); +// sendMessage(msg); } quint64 UAS::getUnixTime(quint64 time) @@ -1589,19 +1596,21 @@ void UAS::requestParameters() void UAS::writeParametersToStorage() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); - //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); +// //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); +// sendMessage(msg); } void UAS::readParametersFromStorage() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); +// sendMessage(msg); } void UAS::enableAllDataTransmission(int rate) @@ -1894,10 +1903,10 @@ void UAS::setSystemType(int systemType) { switch (systemType) { - case MAV_FIXED_WING: + case MAV_TYPE_FIXED_WING: airframe = QGC_AIRFRAME_EASYSTAR; break; - case MAV_QUADROTOR: + case MAV_TYPE_QUADROTOR: airframe = QGC_AIRFRAME_MIKROKOPTER; break; } @@ -1945,31 +1954,19 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float sendMessage(msg); } -/** - * Sets an action - * - **/ -void UAS::setAction(MAV_ACTION action) -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action); - // Send message twice to increase chance that it reaches its goal - sendMessage(msg); - sendMessage(msg); -} - /** * Launches the system * **/ void UAS::launch() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** @@ -1978,12 +1975,12 @@ void UAS::launch() **/ void UAS::enable_motors() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); +// mavlink_message_t msg; +// // FIXME MAVLINKV10PORTINGNEEDED +// //mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** @@ -1992,12 +1989,12 @@ void UAS::enable_motors() **/ void UAS::disable_motors() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); +// mavlink_message_t msg; +// // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust) @@ -2113,33 +2110,36 @@ void UAS::clearWaypointList() void UAS::halt() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } void UAS::go() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_CONTINUE); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_CONTINUE); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** Order the robot to return home / to land on the runway **/ void UAS::home() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_RETURN); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_RETURN); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** @@ -2148,96 +2148,102 @@ void UAS::home() */ void UAS::emergencySTOP() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// // FIXME MAVLINKV10PORTINGNEEDED +// //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** - * All systems are immediately shut down (e.g. the main power line is cut). + * Shut down this mav - All onboard systems are immediately shut down (e.g. the main power line is cut). * @warning This might lead to a crash * * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards */ bool UAS::emergencyKILL() { - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); - msgBox.setInformativeText("Do you want to cut power on all systems?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - - if (ret == QMessageBox::Yes) - { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); - result = true; - } - return result; + // FIXME MAVLINKV10PORTINGNEEDED +// bool result = false; +// QMessageBox msgBox; +// msgBox.setIcon(QMessageBox::Critical); +// msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); +// msgBox.setInformativeText("Do you want to cut power on all systems?"); +// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); +// msgBox.setDefaultButton(QMessageBox::Cancel); +// int ret = msgBox.exec(); + +// // Close the message box shortly after the click to prevent accidental clicks +// QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + +// if (ret == QMessageBox::Yes) +// { +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); +// result = true; +// } +// return result; + return false; } void UAS::startHil() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_START_HILSIM); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_START_HILSIM); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } void UAS::stopHil() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_STOP_HILSIM); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_STOP_HILSIM); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } void UAS::shutdown() { - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Shutting down the UAS"); - msgBox.setInformativeText("Do you want to shut down the onboard computer?"); - - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - if (ret == QMessageBox::Yes) - { - // If the active UAS is set, execute command - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); - - result = true; - } + // FIXME MAVLINKV10PORTINGNEEDED +// bool result = false; +// QMessageBox msgBox; +// msgBox.setIcon(QMessageBox::Critical); +// msgBox.setText("Shutting down the UAS"); +// msgBox.setInformativeText("Do you want to shut down the onboard computer?"); + +// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); +// msgBox.setDefaultButton(QMessageBox::Cancel); +// int ret = msgBox.exec(); + +// // Close the message box shortly after the click to prevent accidental clicks +// QTimer::singleShot(5000, &msgBox, SLOT(reject())); + +// if (ret == QMessageBox::Yes) +// { +// // If the active UAS is set, execute command +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); + +// result = true; +// } } void UAS::setTargetPosition(float x, float y, float z, float yaw) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d958cb66ac..17bb076f96 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -253,8 +253,6 @@ public slots: } /** @brief Set a new name **/ void setUASName(const QString& name); - /** @brief Executes an action **/ - void setAction(MAV_ACTION action); /** @brief Executes a command **/ void executeCommand(MAV_CMD command); /** @brief Executes a command **/ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 56ac5383fd..76707ceb73 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -188,8 +188,6 @@ public slots: /** @brief Set a new name for the system */ virtual void setUASName(const QString& name) = 0; - /** @brief Sets an action **/ - virtual void setAction(MAV_ACTION action) = 0; /** @brief Execute command immediately **/ virtual void executeCommand(MAV_CMD command) = 0; /** @brief Executes a command **/ diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index c541cb3f93..5572cded1e 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -586,8 +586,10 @@ int UASWaypointManager::getLocalFrameCount() // Search through all waypoints, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) { - if (p->getFrame() == MAV_FRAME_GLOBAL) { + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_GLOBAL) + { i++; } } @@ -600,8 +602,10 @@ int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp) // Search through all waypoints, // counting only those in local frame int i = 0; - foreach (Waypoint* p, waypoints) { - if (p->getFrame() == MAV_FRAME_LOCAL) { + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU) + { if (p == wp) { return i; } @@ -617,7 +621,8 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) // Search through all waypoints, // counting only those in mission frame int i = 0; - foreach (Waypoint* p, waypoints) { + foreach (Waypoint* p, waypoints) + { if (p->getFrame() == MAV_FRAME_MISSION) { if (p == wp) { return i; diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index f3fab5debd..c3b050d72e 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -355,7 +355,7 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) for (int j = 0; j < len; j++) { unsigned char byte = bytes.at(j); - // Filter MAVLink (http://pixhawk.ethz.ch/wiki/mavlink/) messages out of the stream. + // Filter MAVLink (http://qgroundcontrol.org/mavlink/) messages out of the stream. if (filterMAVLINK) { if (this->bytesToIgnore > 0) diff --git a/thirdParty/mavlink/include/protocol.h b/thirdParty/mavlink/include/protocol.h index e9cca2e22c..c69a872cc3 100644 --- a/thirdParty/mavlink/include/protocol.h +++ b/thirdParty/mavlink/include/protocol.h @@ -6,10 +6,6 @@ #include "checksum.h" -#ifdef MAVLINK_CHECK_LENGTH -extern const uint8_t mavlink_msg_lengths[256]; -#endif - /** * @brief Initialize the communication stack * @@ -439,521 +435,4 @@ static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint #define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) #endif -#define FILE_FINISHED - -#ifndef FILE_FINISHED -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ - -#define MAVLINK_PACKET_START_CANDIDATES 50 -/* -static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; - #else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; - static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; - #endif - - // Set a packet start candidate index if sign is start sign - if (c == MAVLINK_STX) - { - m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; - } - - // Parse normally, if a CRC mismatch occurs retry with the next packet index -} -//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; -//#else -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; -//#endif -//// Initializes only once, values keep unchanged after first initialization -// mavlink_parse_state_initialize(&m_mavlink_status[chan]); -// -//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message -//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status -//int bufferIndex = 0; -// -//status->msg_received = 0; -// -//switch (status->parse_state) -//{ -//case MAVLINK_PARSE_STATE_UNINIT: -//case MAVLINK_PARSE_STATE_IDLE: -// if (c == MAVLINK_STX) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; -// mavlink_start_checksum(rxmsg); -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_STX: -// if (status->msg_received) -// { -// status->buffer_overrun++; -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 -// rxmsg->len = c; -// status->packet_idx = 0; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_LENGTH: -// rxmsg->seq = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SEQ: -// rxmsg->sysid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SYSID: -// rxmsg->compid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_COMPID: -// rxmsg->msgid = c; -// mavlink_update_checksum(rxmsg, c); -// if (rxmsg->len == 0) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// else -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_MSGID: -// rxmsg->payload[status->packet_idx++] = c; -// mavlink_update_checksum(rxmsg, c); -// if (status->packet_idx == rxmsg->len) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -// if (c != rxmsg->ck_a) -// { -// // Check first checksum byte -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// 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; -// } -// 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_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; -} - -r_mavlink_status->current_seq = status->current_seq+1; -r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; -r_mavlink_status->packet_rx_drop_count = status->parse_error; -return status->msg_received; -} - */ - - -typedef union __generic_16bit -{ - uint8_t b[2]; - int16_t s; -} generic_16bit; - -typedef union __generic_32bit -{ - uint8_t b[4]; - float f; - int32_t i; - int16_t s; -} generic_32bit; - -typedef union __generic_64bit -{ - uint8_t b[8]; - int64_t ll; ///< Long long (64 bit) -} generic_64bit; - -/** - * @brief Place an unsigned byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = b; - return sizeof(b); -} - -/** - * @brief Place a signed byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = (uint8_t)b; - return sizeof(b); -} - -/** - * @brief Place two unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>8)&0xff; - buffer[bindex+1] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place two signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint16_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>56)&0xff; - buffer[bindex+1] = (b>>48)&0xff; - buffer[bindex+2] = (b>>40)&0xff; - buffer[bindex+3] = (b>>32)&0xff; - buffer[bindex+4] = (b>>24)&0xff; - buffer[bindex+5] = (b>>16)&0xff; - buffer[bindex+6] = (b>>8)&0xff; - buffer[bindex+7] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint64_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place a float into the buffer - * - * @param b the float to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) -{ - generic_32bit g; - g.f = b; - return put_int32_t_by_index(g.i, bindex, buffer); -} - -/** - * @brief Place an array into the buffer - * - * @param b the array to add - * @param length size of the array (for strings: length WITH '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) -{ - memcpy(buffer+bindex, b, length); - return length; -} - -/** - * @brief Place a string into the buffer - * - * @param b the string to add - * @param maxlength size of the array (for strings: length WITHOUT '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) -{ - uint16_t length = 0; - // Copy string into buffer, ensuring not to exceed the buffer size - int i; - for (i = 1; i < maxlength; i++) - { - length++; - // String characters - if (i < (maxlength - 1)) - { - buffer[bindex+i] = b[i]; - // Stop at null character - if (b[i] == '\0') - { - break; - } - } - // Enforce null termination at end of buffer - else if (i == (maxlength - 1)) - { - buffer[i] = '\0'; - } - } - // Write length into first field - put_uint8_t_by_index(length, bindex, buffer); - return length; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - generic_32bit bin; - generic_32bit bout; - uint8_t i_bit_index, i_byte_index, curr_bits_n; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (8 - i_bit_index)) - { - // Enough space - curr_bits_n = bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} -*/ -#endif /* FILE_FINISHED */ - #endif /* _MAVLINK_PROTOCOL_H_ */ -- GitLab From 06be9881336a6f420b022bf3d94f3de66cd15c08 Mon Sep 17 00:00:00 2001 From: lm Date: Sun, 31 Jul 2011 18:53:47 +0200 Subject: [PATCH 008/131] Finished porting, successfully tested with UDP MAVLink v1.0 client --- qgroundcontrol.pro | 2 - src/Waypoint.cc | 39 +++-- src/comm/MAVLinkProtocol.cc | 2 +- src/ui/HSIDisplay.cc | 3 +- src/ui/MainWindow.cc | 8 +- .../RadioCalibrationWindow.cc | 15 +- src/ui/WaypointView.cc | 6 +- src/ui/designer/QGCActionButton.cc | 136 ------------------ src/ui/designer/QGCActionButton.h | 34 ----- src/ui/designer/QGCToolWidget.cc | 17 +-- src/ui/designer/QGCToolWidget.h | 2 - 11 files changed, 45 insertions(+), 219 deletions(-) delete mode 100644 src/ui/designer/QGCActionButton.cc delete mode 100644 src/ui/designer/QGCActionButton.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 7b645d1b79..9d39bdfb41 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -299,7 +299,6 @@ HEADERS += src/MG.h \ src/ui/uas/QGCUnconnectedInfoWidget.h \ src/ui/designer/QGCToolWidget.h \ src/ui/designer/QGCParamSlider.h \ - src/ui/designer/QGCActionButton.h \ src/ui/designer/QGCCommandButton.h \ src/ui/designer/QGCToolWidgetItem.h \ src/ui/QGCMAVLinkLogPlayer.h \ @@ -425,7 +424,6 @@ SOURCES += src/main.cc \ src/ui/uas/QGCUnconnectedInfoWidget.cc \ src/ui/designer/QGCToolWidget.cc \ src/ui/designer/QGCParamSlider.cc \ - src/ui/designer/QGCActionButton.cc \ src/ui/designer/QGCCommandButton.cc \ src/ui/designer/QGCToolWidgetItem.cc \ src/ui/QGCMAVLinkLogPlayer.cc \ diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 2fd5a7099f..bfcde0c4c1 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -109,7 +109,8 @@ void Waypoint::setId(quint16 id) void Waypoint::setX(double x) { - if (this->x != x && (this->frame == MAV_FRAME_LOCAL)) { + if (!isinf(x) && !isnan(x) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU))) + { this->x = x; emit changed(this); } @@ -117,7 +118,8 @@ void Waypoint::setX(double x) void Waypoint::setY(double y) { - if (this->y != y && (this->frame == MAV_FRAME_LOCAL)) { + if (!isinf(y) && !isnan(y) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU))) + { this->y = y; emit changed(this); } @@ -125,7 +127,8 @@ void Waypoint::setY(double y) void Waypoint::setZ(double z) { - if (this->z != z && (this->frame == MAV_FRAME_LOCAL)) { + if (!isinf(z) && !isnan(z) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU))) + { this->z = z; emit changed(this); } @@ -133,7 +136,8 @@ void Waypoint::setZ(double z) void Waypoint::setLatitude(double lat) { - if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) { + if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + { this->x = lat; emit changed(this); } @@ -141,7 +145,8 @@ void Waypoint::setLatitude(double lat) void Waypoint::setLongitude(double lon) { - if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) { + if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + { this->y = lon; emit changed(this); } @@ -149,7 +154,8 @@ void Waypoint::setLongitude(double lon) void Waypoint::setAltitude(double altitude) { - if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) { + if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + { this->z = altitude; emit changed(this); } @@ -157,7 +163,8 @@ void Waypoint::setAltitude(double altitude) void Waypoint::setYaw(int yaw) { - if (this->yaw != yaw) { + if (this->yaw != yaw) + { this->yaw = yaw; emit changed(this); } @@ -165,7 +172,8 @@ void Waypoint::setYaw(int yaw) void Waypoint::setYaw(double yaw) { - if (this->yaw != yaw) { + if (this->yaw != yaw) + { this->yaw = yaw; emit changed(this); } @@ -173,7 +181,8 @@ void Waypoint::setYaw(double yaw) void Waypoint::setAction(int action) { - if (this->action != (MAV_CMD)action) { + if (this->action != (MAV_CMD)action) + { this->action = (MAV_CMD)action; emit changed(this); } @@ -205,7 +214,8 @@ void Waypoint::setAutocontinue(bool autoContinue) void Waypoint::setCurrent(bool current) { - if (this->current != current) { + if (this->current != current) + { this->current = current; emit changed(this); } @@ -213,7 +223,8 @@ void Waypoint::setCurrent(bool current) void Waypoint::setAcceptanceRadius(double radius) { - if (this->param2 != radius) { + if (this->param2 != radius) + { this->param2 = radius; emit changed(this); } @@ -223,7 +234,8 @@ void Waypoint::setParam1(double param1) { //qDebug() << "SENDER:" << QObject::sender(); //qDebug() << "PARAM1 SET REQ:" << param1; - if (this->param1 != param1) { + if (this->param1 != param1) + { this->param1 = param1; emit changed(this); } @@ -231,7 +243,8 @@ void Waypoint::setParam1(double param1) void Waypoint::setParam2(double param2) { - if (this->param2 != param2) { + if (this->param2 != param2) + { this->param2 = param2; emit changed(this); } diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 69ee72af4d..098fe65ffa 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -395,7 +395,7 @@ void MAVLinkProtocol::sendHeartbeat() { if (m_heartbeatsEnabled) { mavlink_message_t beat; - mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, OCU, MAV_AUTOPILOT_GENERIC); + mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_OCU, MAV_CLASS_INVALID); sendMessage(beat); } if (m_authEnabled) { diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 27f40b8498..bc3e193d3a 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -792,7 +792,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter) for (int i = 0; i < list.size(); i++) { QPointF in; - if (list.at(i)->getFrame() == MAV_FRAME_LOCAL) { + if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED) + { // Do not transform in = QPointF(list.at(i)->getX(), list.at(i)->getY()); } else { diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 1f39e4a270..14d8c4c2ad 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1629,7 +1629,7 @@ void MainWindow::UASCreated(UASInterface* uas) } switch (uas->getAutopilotType()) { - case (MAV_AUTOPILOT_SLUGS): { + case (MAV_CLASS_SLUGS): { // Build Slugs Widgets buildSlugsWidgets(); @@ -1667,9 +1667,9 @@ void MainWindow::UASCreated(UASInterface* uas) } break; default: - case (MAV_AUTOPILOT_GENERIC): - case (MAV_AUTOPILOT_ARDUPILOTMEGA): - case (MAV_AUTOPILOT_PIXHAWK): { + case (MAV_CLASS_GENERIC): + case (MAV_CLASS_ARDUPILOTMEGA): + case (MAV_CLASS_PIXHAWK): { // Build Pixhawk Widgets buildPxWidgets(); diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.cc b/src/ui/RadioCalibration/RadioCalibrationWindow.cc index 21ae680661..5933f7e3df 100644 --- a/src/ui/RadioCalibration/RadioCalibrationWindow.cc +++ b/src/ui/RadioCalibration/RadioCalibrationWindow.cc @@ -270,13 +270,14 @@ void RadioCalibrationWindow::send() void RadioCalibrationWindow::request() { - qDebug() << __FILE__ << __LINE__ << "READ FROM UAV"; - UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId)); - if (uas) { - mavlink_message_t msg; - mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC); - uas->sendMessage(msg); - } + // FIXME MAVLINKV10PORTINGNEEDED +// qDebug() << __FILE__ << __LINE__ << "READ FROM UAV"; +// UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId)); +// if (uas) { +// mavlink_message_t msg; +// mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC); +// uas->sendMessage(msg); +// } } void RadioCalibrationWindow::receive(const QPointer& radio) diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 9d157ca142..516b68e3f5 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -52,7 +52,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : // add frames m_ui->comboBox_frame->addItem("Abs. Alt/Global",MAV_FRAME_GLOBAL); m_ui->comboBox_frame->addItem("Rel. Alt/Global", MAV_FRAME_GLOBAL_RELATIVE_ALT); - m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL); + m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL_NED); m_ui->comboBox_frame->addItem("Mission",MAV_FRAME_MISSION); // Initialize view correctly @@ -322,7 +322,7 @@ void WaypointView::updateFrameView(int frame) m_ui->comboBox_frame->show(); m_ui->customActionWidget->hide(); break; - case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_NED: m_ui->lonSpinBox->hide(); m_ui->latSpinBox->hide(); m_ui->altSpinBox->hide(); @@ -386,7 +386,7 @@ void WaypointView::updateValues() updateFrameView(frame); } switch(frame) { - case MAV_FRAME_LOCAL: { + case MAV_FRAME_LOCAL_NED: { if (m_ui->posNSpinBox->value() != wp->getX()) { m_ui->posNSpinBox->setValue(wp->getX()); } diff --git a/src/ui/designer/QGCActionButton.cc b/src/ui/designer/QGCActionButton.cc deleted file mode 100644 index 72753e91ff..0000000000 --- a/src/ui/designer/QGCActionButton.cc +++ /dev/null @@ -1,136 +0,0 @@ -#include "QGCActionButton.h" -#include "ui_QGCActionButton.h" - -#include "MAVLinkProtocol.h" -#include "UASManager.h" - -const char* kActionLabels[MAV_ACTION_NB] = { - "HOLD", - "START MOTORS", - "LAUNCH", - "RETURN", - "EMERGENCY LAND", - "EMERGENCY KILL", - "CONFIRM KILL", - "CONTINUE", - "STOP MOTORS", - "HALT", - "SHUTDOWN", - "REBOOT", - "SET MANUAL", - "SET AUTO", - "READ STORAGE", - "WRITE STORAGE", - "CALIBRATE RC", - "CALIBRATE GYRO", - "CALIBRATE MAG", - "CALIBRATE ACC", - "CALIBRATE PRESSURE", - "START REC", - "PAUSE REC", - "STOP REC", - "TAKEOFF", - "NAVIGATE", - "LAND", - "LOITER", - "SET ORIGIN", - "RELAY ON", -//"RELAY OFF", -//"GET IMAGE", -//"START VIDEO", -//"STOP VIDEO", - "RESET MAP", - "RESET PLAN" -}; - -QGCActionButton::QGCActionButton(QWidget *parent) : - QGCToolWidgetItem("Button", parent), - ui(new Ui::QGCActionButton), - uas(NULL) -{ - ui->setupUi(this); - - connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); - connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); - connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); - connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); - - // Hide all edit items - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // add action labels to combobox - for (int i = 0; i < MAV_ACTION_NB; i++) { - ui->editActionComboBox->addItem(kActionLabels[i]); - } -} - -QGCActionButton::~QGCActionButton() -{ - delete ui; -} - -void QGCActionButton::sendAction() -{ - if (QGCToolWidgetItem::uas) { - MAV_ACTION action = static_cast( - ui->editActionComboBox->currentIndex()); - - QGCToolWidgetItem::uas->setAction(action); - } else { - qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; - } -} - -void QGCActionButton::setActionButtonName(QString text) -{ - ui->actionButton->setText(text); -} - -void QGCActionButton::startEditMode() -{ - ui->editActionComboBox->show(); - ui->editActionsRefreshButton->show(); - ui->editFinishButton->show(); - ui->editNameLabel->show(); - ui->editButtonName->show(); - isInEditMode = true; -} - -void QGCActionButton::endEditMode() -{ - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // Write to settings - emit editingFinished(); - - isInEditMode = false; -} - -void QGCActionButton::writeSettings(QSettings& settings) -{ - settings.setValue("TYPE", "BUTTON"); - settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text()); - settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text()); - settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex()); - settings.sync(); -} - -void QGCActionButton::readSettings(const QSettings& settings) -{ - ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - - ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - qDebug() << "DONE READING SETTINGS"; -} diff --git a/src/ui/designer/QGCActionButton.h b/src/ui/designer/QGCActionButton.h deleted file mode 100644 index a145e9a00b..0000000000 --- a/src/ui/designer/QGCActionButton.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef QGCACTIONBUTTON_H -#define QGCACTIONBUTTON_H - -#include "QGCToolWidgetItem.h" - -namespace Ui -{ -class QGCActionButton; -} - -class UASInterface; - -class QGCActionButton : public QGCToolWidgetItem -{ - Q_OBJECT - -public: - explicit QGCActionButton(QWidget *parent = 0); - ~QGCActionButton(); - -public slots: - void sendAction(); - void setActionButtonName(QString text); - void startEditMode(); - void endEditMode(); - void writeSettings(QSettings& settings); - void readSettings(const QSettings& settings); - -private: - Ui::QGCActionButton *ui; - UASInterface* uas; -}; - -#endif // QGCACTIONBUTTON_H diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index 6a0f7678ca..9a3fb8ec26 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -11,7 +11,6 @@ #include #include "QGCParamSlider.h" -#include "QGCActionButton.h" #include "QGCCommandButton.h" #include "UASManager.h" @@ -129,10 +128,7 @@ void QGCToolWidget::loadSettings(QSettings& settings) QString type = settings.value("TYPE", "UNKNOWN").toString(); if (type != "UNKNOWN") { QGCToolWidgetItem* item = NULL; - if (type == "BUTTON") { - item = new QGCActionButton(this); - qDebug() << "CREATED BUTTON"; - } else if (type == "COMMANDBUTTON") { + if (type == "COMMANDBUTTON") { item = new QGCCommandButton(this); qDebug() << "CREATED COMMANDBUTTON"; } else if (type == "SLIDER") { @@ -303,17 +299,6 @@ void QGCToolWidget::addParam() slider->startEditMode(); } -void QGCToolWidget::addAction() -{ - QGCActionButton* button = new QGCActionButton(this); - if (ui->hintLabel) { - ui->hintLabel->deleteLater(); - ui->hintLabel = NULL; - } - toolLayout->addWidget(button); - button->startEditMode(); -} - void QGCToolWidget::addCommand() { QGCCommandButton* button = new QGCCommandButton(this); diff --git a/src/ui/designer/QGCToolWidget.h b/src/ui/designer/QGCToolWidget.h index 7d8b74dadc..81c9cf4eb1 100644 --- a/src/ui/designer/QGCToolWidget.h +++ b/src/ui/designer/QGCToolWidget.h @@ -82,8 +82,6 @@ protected: protected slots: void addParam(); - /** @deprecated */ - void addAction(); void addCommand(); void setTitle(); void setTitle(QString title); -- GitLab From 5ad0a12362b8e6a4b144ccde0af1902b4cbbbdb1 Mon Sep 17 00:00:00 2001 From: lm Date: Mon, 1 Aug 2011 17:08:24 +0200 Subject: [PATCH 009/131] Cleanup of code generator, working towards field hashes --- .../generator/MAVLinkXMLParserV10.cc | 242 ++++++++++-------- .../generator/MAVLinkXMLParserV10.h | 6 + 2 files changed, 142 insertions(+), 106 deletions(-) diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc index 1ef314a4b9..4b78f9fc67 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc @@ -62,6 +62,41 @@ MAVLinkXMLParserV10::~MAVLinkXMLParserV10() { } +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +void MAVLinkXMLParserV10::crcAccumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +} + +/** + * @param crcAccum the 16 bit X.25 CRC + */ +void MAVLinkXMLParserV10::crcInit(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + + /** * Generate C-code (C-89 compliant) out of the XML protocol specs. */ @@ -171,26 +206,6 @@ bool MAVLinkXMLParserV10::generate() includeParser.generate(); mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n"; - - // OLD MODE: MERGE BOTH FILES - // const QString instanceText(QString::fromUtf8(file.readAll())); - // includeDoc.setContent(instanceText); - // // Get all messages - // QDomNode in = includeDoc.documentElement().firstChild(); - // QDomElement ie = in.toElement(); - // if (!ie.isNull()) - // { - // if (ie.tagName() == "messages" || ie.tagName() == "include") - // { - // QDomNode ref = n.parentNode().insertAfter(in, n); - // if (ref.isNull()) - // { - // emit parseState(QString("ERROR: Inclusion failed: XML syntax error in file %1. Wrong/misspelled XML?\nAbort.").arg(fileName)); - // return false; - // } - // } - // } - emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); } else @@ -311,11 +326,27 @@ bool MAVLinkXMLParserV10::generate() QDomElement pp2 = pp.toElement(); if (pp2.isText() || pp2.isCDATASection()) { - fieldComment += pp2.nodeValue() + sep; + // If this is the only field, don't add the separator + if (pp.nextSibling().isNull()) + { + fieldComment += pp2.nodeValue(); + } + else + { + fieldComment += pp2.nodeValue() + sep; + } } else if (pp2.isElement()) { - fieldComment += pp2.text() + sep; + // If this is the only field, don't add the separator + if (pp.nextSibling().isNull()) + { + fieldComment += pp2.text(); + } + else + { + fieldComment += pp2.text() + sep; + } } pp = pp.nextSibling(); } @@ -430,6 +461,13 @@ bool MAVLinkXMLParserV10::generate() // Get the message fields QDomNode f = e.firstChild(); + + // The field types and order are hashed with a checksum + + // Initialize CRC + uint16_t fieldHash; + crcInit(&fieldHash); + while (!f.isNull()) { QDomElement e2 = f.toElement(); @@ -483,68 +521,17 @@ bool MAVLinkXMLParserV10::generate() } } - // Array handling is different from simple types + // ARRAYS are not longer supported - leave error message in here to inform users! else if (fieldType.startsWith("array")) { - int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); - QString arrayType = fieldType.split("[").first(); - if (arrayType.contains("array")) calculatedLength += arrayLength; else - if (arrayType.contains("char")) calculatedLength += arrayLength; else - if (arrayType.contains("int8")) calculatedLength += arrayLength; else - if (arrayType.contains("int16")) calculatedLength += arrayLength*2; else - if (arrayType.contains("int32")) calculatedLength += arrayLength*4; else - if (arrayType.contains("int64")) calculatedLength += arrayLength*8; else - if (arrayType == "float") calculatedLength += arrayLength*4; else { - emit parseState(tr("ERROR: In message %1 inavlid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); - return false; - } - packParameters += QString(", const ") + QString("int8_t*") + " " + fieldName; - packArguments += ", " + messageName + "->" + fieldName; - - // Add field to C structure - cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("int8_t", fieldName, QString::number(arrayLength), fieldText); - // Add pack line to message_xx_pack function - // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); - packLines += QString("\tmemcpy( p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); - // Add decode function for this type - decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); - - if (fieldOffset != "") { // does not use the number - always moves up one slot - QStringList itemList; - // Swap field in C structure - itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; - cStructLines = itemList.join("\n") + "\n"; - - // Swap line in message_xx_pack function - itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; - packLines = itemList.join("\n") + "\n"; - - // Swap line in decode function for this type - itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; - decodeLines = itemList.join("\n") + "\n"; - } - - arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + emit parseState(tr("ERROR: In message %1 deprecated type used near line %2 of file %3. Please change from array[size] to uint8_t[size] to get the same behaviour.\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); + return false; } else if (fieldType.startsWith("string")) { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + // String array is always unsigned char, so bytes calculatedLength += arrayLength; - QString arrayType = fieldType.split("[").first(); - if (arrayType.contains("string")) calculatedLength += arrayLength; else - if (arrayType.contains("array")) calculatedLength += arrayLength; else - if (arrayType.contains("char")) calculatedLength += arrayLength; else - if (arrayType.contains("int8")) calculatedLength += arrayLength; else - if (arrayType.contains("int16")) calculatedLength += arrayLength*2; else - if (arrayType.contains("int32")) calculatedLength += arrayLength*4; else - if (arrayType.contains("int64")) calculatedLength += arrayLength*8; else - if (arrayType == "float") calculatedLength += arrayLength*4; else { - emit parseState(tr("ERROR: In message %1 inavlid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); - return false; - } packParameters += QString(", const ") + QString("char*") + " " + fieldName; packArguments += ", " + messageName + "->" + fieldName; @@ -557,21 +544,22 @@ bool MAVLinkXMLParserV10::generate() decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - if (fieldOffset != "") { // does not use the number - always moves up one slot + if (fieldOffset != "") + { // does not use the number - always moves up one slot QStringList itemList; // Swap field in C structure itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); cStructLines = itemList.join("\n") + "\n"; // Swap line in message_xx_pack function itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); packLines = itemList.join("\n") + "\n"; // Swap line in decode function for this type itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); decodeLines = itemList.join("\n") + "\n"; } } @@ -580,16 +568,39 @@ bool MAVLinkXMLParserV10::generate() { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); - if (arrayType.contains("array")) calculatedLength += arrayLength; else - if (arrayType.contains("char")) calculatedLength += arrayLength; else - if (arrayType.contains("int8")) calculatedLength += arrayLength; else - if (arrayType.contains("int16")) calculatedLength += arrayLength*2; else - if (arrayType.contains("int32")) calculatedLength += arrayLength*4; else - if (arrayType.contains("int64")) calculatedLength += arrayLength*8; else - if (arrayType == "float") calculatedLength += arrayLength*4; else { - emit parseState(tr("ERROR: In message %1 inavlid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); - return false; - } + if (arrayType.contains("array")) + { + calculatedLength += arrayLength; + } + else if (arrayType.contains("char")) + { + calculatedLength += arrayLength; + } + else if (arrayType.contains("int8")) + { + calculatedLength += arrayLength; + } + else if (arrayType.contains("int16")) + { + calculatedLength += arrayLength*2; + } + else if (arrayType.contains("int32")) + { + calculatedLength += arrayLength*4; + } + else if (arrayType.contains("int64")) + { + calculatedLength += arrayLength*8; + } + else if (arrayType == "float") + { + calculatedLength += arrayLength*4; + } + else + { + emit parseState(tr("ERROR: In message %1 invalid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); + return false; + } packParameters += QString(", const ") + arrayType + "* " + fieldName; packArguments += ", " + messageName + "->" + fieldName; @@ -601,21 +612,22 @@ bool MAVLinkXMLParserV10::generate() // Add decode function for this type decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); - if (fieldOffset != "") { // does not use the number - always moves up one slot + if (fieldOffset != "") + { // does not use the number - always moves up one slot QStringList itemList; // Swap field in C structure itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); cStructLines = itemList.join("\n") + "\n"; // Swap line in message_xx_pack function itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); packLines = itemList.join("\n") + "\n"; // Swap line in decode function for this type itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); decodeLines = itemList.join("\n") + "\n"; } @@ -662,17 +674,35 @@ bool MAVLinkXMLParserV10::generate() if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; decodeLines = itemList.join("\n") + "\n"; } - - if (fieldType.contains("array")) calculatedLength += 1; else - if (fieldType.contains("char")) calculatedLength += 1; else - if (fieldType.contains("int8")) calculatedLength += 1; else - if (fieldType.contains("int16")) calculatedLength += 2; else - if (fieldType.contains("int32")) calculatedLength += 4; else - if (fieldType.contains("int64")) calculatedLength += 8; else - if (fieldType == "float") calculatedLength += 4; else { - emit parseState(tr("ERROR: In message %1 inavlid type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, fieldType)); - return false; - } + if (fieldType.contains("char")) + { + calculatedLength += 1; + } + else if (fieldType.contains("int8")) + { + calculatedLength += 1; + } + else if (fieldType.contains("int16")) + { + calculatedLength += 2; + } + else if (fieldType.contains("int32")) + { + calculatedLength += 4; + } + else if (fieldType.contains("int64")) + { + calculatedLength += 8; + } + else if (fieldType == "float") + { + calculatedLength += 4; + } + else + { + emit parseState(tr("ERROR: In message %1 inavlid type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, fieldType)); + return false; + } } diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h index 0b17e5ad83..3cf2faa7e2 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h @@ -58,6 +58,12 @@ signals: void parseState(QString message); protected: + /** @brief Accumulate the X.25 CRC by adding one char at a time. */ + void crcAccumulate(uint8_t data, uint16_t *crcAccum); + + /** @brief Initialize the buffer for the X.25 CRC */ + void crcInit(uint16_t* crcAccum); + QDomDocument* doc; QString outputDirName; QString fileName; -- GitLab From bd8a2f77713c0b9d27688121e0efedc7b61d3ae3 Mon Sep 17 00:00:00 2001 From: lm Date: Tue, 2 Aug 2011 23:15:22 +0200 Subject: [PATCH 010/131] Removed bogus AS4 proto place holders --- src/comm/AS4Protocol.cc | 154 --------------------------------------- src/comm/AS4Protocol.h | 155 ---------------------------------------- 2 files changed, 309 deletions(-) delete mode 100644 src/comm/AS4Protocol.cc delete mode 100644 src/comm/AS4Protocol.h diff --git a/src/comm/AS4Protocol.cc b/src/comm/AS4Protocol.cc deleted file mode 100644 index 5e0c560025..0000000000 --- a/src/comm/AS4Protocol.cc +++ /dev/null @@ -1,154 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Brief Description - * - * @author Lorenz Meier - * - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "QGC.h" - - -AS4Protocol::AS4Protocol() -{ - - // Start heartbeat timer, emitting a heartbeat at the configured rate - heartbeatRate = 1; ///< SAE AS-4 has a fixed heartbeat rate of 1 hz. -// heartbeatTimer = new QTimer(this); -// connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT()); -// heartbeatTimer->start(1000/heartbeatRate); - /* - // Start the node manager - configData = new FileLoader("nodeManager.conf"); - handler = new MyHandler(); - - try - { - nodeManager = new NodeManager(configData, handler); - qDebug() << "SAE AS-4 NODE MANAGER constructed"; - } - catch(char *exceptionString) - { - printf("%s", exceptionString); - printf("Terminating Program...\n"); - } - catch(...) - { - printf("Node Manager Construction Failed. Terminating Program...\n"); - } - */ - -} - -AS4Protocol::~AS4Protocol() -{ -// delete nodeManager; -// delete handler; -// delete configData; -} - -void AS4Protocol::run() -{ - forever { - QGC::SLEEP::msleep(5000); - } -} - - -/** - * @brief Receive bytes from a communication interface. - * - * The bytes copied by calling the LinkInterface::readBytes() method. - * - * @param link The interface to read from - * @see LinkInterface - **/ -void AS4Protocol::receiveBytes(LinkInterface* link) -{ -// receiveMutex.lock(); - // Prepare buffer - //static const int maxlen = 4096 * 100; - //static char buffer[maxlen]; - - qint64 bytesToRead = link->bytesAvailable(); - - // Get all data at once, let link read the bytes in the buffer array - //link->readBytes(buffer, maxlen); -// -// /* -// // Debug output -// std::cerr << "receive buffer: "; -// for (int i = 0; i < bytesToRead; i++) -// { -// std::cerr << std::hex << static_cast(buffer[i]); -// } -// std::cerr << std::dec << " length: " << bytesToRead; -// */ -// -// qDebug() << __FILE__ << __LINE__ << ": buffer size:" << maxlen << "bytes:" << bytesToRead; -// - - for (int position = 0; position < bytesToRead; position++) { - } -// receiveMutex.unlock(); - -} - - -/** - * @brief Get the human-readable name of this protocol. - * - * @return The name of this protocol - **/ -QString AS4Protocol::getName() -{ - return QString(tr("SAE AS-4")); -} - -void AS4Protocol::setHeartbeatRate(int rate) -{ - heartbeatRate = rate; - heartbeatTimer->setInterval(1000/heartbeatRate); -} - -int AS4Protocol::getHeartbeatRate() -{ - return heartbeatRate; -} diff --git a/src/comm/AS4Protocol.h b/src/comm/AS4Protocol.h deleted file mode 100644 index 00a42df5bf..0000000000 --- a/src/comm/AS4Protocol.h +++ /dev/null @@ -1,155 +0,0 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - PIXHAWK is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with PIXHAWK. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Brief Description - * - * @author Lorenz Meier - * - */ - -#ifndef AS4PROTOCOL_H_ -#define AS4PROTOCOL_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -/*#include - -class MyHandler : public EventHandler -{ -public: - ~MyHandler() - { - - } - - void handleEvent(NodeManagerEvent *e) - { - SystemTreeEvent *treeEvent; - ErrorEvent *errorEvent; - JausMessageEvent *messageEvent; - DebugEvent *debugEvent; - ConfigurationEvent *configEvent; - - switch(e->getType()) - { - case NodeManagerEvent::SystemTreeEvent: - treeEvent = (SystemTreeEvent *)e; - printf("%s\n", treeEvent->toString().c_str()); - delete e; - break; - - case NodeManagerEvent::ErrorEvent: - errorEvent = (ErrorEvent *)e; - printf("%s\n", errorEvent->toString().c_str()); - delete e; - break; - - case NodeManagerEvent::JausMessageEvent: - messageEvent = (JausMessageEvent *)e; - // If you turn this on, the system gets spam-y this is very useful for debug purposes - if(messageEvent->getJausMessage()->commandCode != JAUS_REPORT_HEARTBEAT_PULSE) - { - //printf("%s\n", messageEvent->toString().c_str()); - } - else - { - //printf("%s\n", messageEvent->toString().c_str()); - } - delete e; - break; - - case NodeManagerEvent::DebugEvent: - debugEvent = (DebugEvent *)e; - //printf("%s\n", debugEvent->toString().c_str()); - delete e; - break; - - case NodeManagerEvent::ConfigurationEvent: - configEvent = (ConfigurationEvent *)e; - printf("%s\n", configEvent->toString().c_str()); - delete e; - break; - - default: - delete e; - break; - } - } -};*/ - -/** - * SAE AS-4 Nodemanager - * - **/ -class AS4Protocol : public ProtocolInterface -{ - Q_OBJECT - -public: - AS4Protocol(); - ~AS4Protocol(); - - void run(); - QString getName(); - int getHeartbeatRate(); - -public slots: - void receiveBytes(LinkInterface* link); - /** - * @brief Set the rate at which heartbeats are emitted - * - * The default rate is 1 Hertz. - * - * @param rate heartbeat rate in hertz (times per second) - */ - void setHeartbeatRate(int rate); - - /** - * @brief Send an extra heartbeat to all connected units - * - * The heartbeat is sent out of order and does not reset the - * periodic heartbeat emission. It will be just sent in addition. - */ -// void sendHeartbeat(); - -protected: - QTimer* heartbeatTimer; - int heartbeatRate; - QMutex receiveMutex; -// NodeManager* nodeManager; -// MyHandler* handler; -// FileLoader* configData; - -signals: - -}; - -#endif // AS4PROTOCOL_H_ -- GitLab From 02ac005a705418b7e59509eb0d5330062b2171ac Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 5 Aug 2011 09:11:44 +0200 Subject: [PATCH 011/131] Removed last includes to AS4Protocol --- qgroundcontrol.pro | 2 -- src/ui/MainWindow.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9d39bdfb41..479767be0b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -235,7 +235,6 @@ HEADERS += src/MG.h \ src/comm/SerialSimulationLink.h \ src/comm/ProtocolInterface.h \ src/comm/MAVLinkProtocol.h \ - src/comm/AS4Protocol.h \ src/ui/CommConfigurationWindow.h \ src/ui/SerialConfigurationWindow.h \ src/ui/MainWindow.h \ @@ -362,7 +361,6 @@ SOURCES += src/main.cc \ src/comm/SerialLink.cc \ src/comm/SerialSimulationLink.cc \ src/comm/MAVLinkProtocol.cc \ - src/comm/AS4Protocol.cc \ src/ui/CommConfigurationWindow.cc \ src/ui/SerialConfigurationWindow.cc \ src/ui/MainWindow.cc \ diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 6b21e2f43f..e0a0d65ad8 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -49,7 +49,6 @@ This file is part of the QGROUNDCONTROL project #include "UASListWidget.h" #include "MAVLinkProtocol.h" #include "MAVLinkSimulationLink.h" -#include "AS4Protocol.h" #include "ObjectDetectionView.h" #include "HUD.h" #include "JoystickWidget.h" @@ -368,7 +367,6 @@ protected: // TODO Should be moved elsewhere, as the protocol does not belong to the UI MAVLinkProtocol* mavlink; - AS4Protocol* as4link; MAVLinkSimulationLink* simulationLink; LinkInterface* udpLink; -- GitLab From d950361a84054a1e61833ee025ff6d2d597fd10c Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 9 Aug 2011 18:24:40 +0200 Subject: [PATCH 012/131] Updated MAVLink v1.0 release candidate sources --- .../generator/MAVLinkXMLParserV10.cc | 174 ++- thirdParty/mavlink/.gitignore | 1 + .../include/ardupilotmega/ardupilotmega.h | 9 +- .../mavlink/include/ardupilotmega/mavlink.h | 7 +- thirdParty/mavlink/include/common/common.h | 213 +-- thirdParty/mavlink/include/common/mavlink.h | 7 +- .../include/common/mavlink_msg_action.h | 27 +- .../include/common/mavlink_msg_action_ack.h | 26 +- .../include/common/mavlink_msg_attitude.h | 31 +- .../mavlink_msg_attitude_controller_output.h | 29 +- .../include/common/mavlink_msg_auth_key.h | 25 +- .../mavlink/include/common/mavlink_msg_boot.h | 25 +- .../mavlink_msg_change_operator_control.h | 28 +- .../mavlink_msg_change_operator_control_ack.h | 27 +- .../include/common/mavlink_msg_command.h | 40 +- .../include/common/mavlink_msg_command_ack.h | 26 +- .../common/mavlink_msg_control_status.h | 32 +- .../include/common/mavlink_msg_debug.h | 28 +- .../include/common/mavlink_msg_debug_vect.h | 31 +- .../include/common/mavlink_msg_full_state.h | 394 ++++++ .../common/mavlink_msg_global_position.h | 31 +- .../common/mavlink_msg_global_position_int.h | 30 +- .../common/mavlink_msg_gps_local_origin_set.h | 27 +- .../include/common/mavlink_msg_gps_raw.h | 35 +- .../include/common/mavlink_msg_gps_raw_int.h | 35 +- .../mavlink_msg_gps_set_global_origin.h | 33 +- .../include/common/mavlink_msg_gps_status.h | 30 +- .../include/common/mavlink_msg_heartbeat.h | 27 +- .../common/mavlink_msg_local_position.h | 31 +- .../mavlink_msg_local_position_setpoint.h | 28 +- .../mavlink_msg_local_position_setpoint_set.h | 34 +- .../common/mavlink_msg_manual_control.h | 35 +- .../common/mavlink_msg_named_value_float.h | 28 +- .../common/mavlink_msg_named_value_int.h | 28 +- .../mavlink_msg_nav_controller_output.h | 38 +- .../common/mavlink_msg_param_request_list.h | 26 +- .../common/mavlink_msg_param_request_read.h | 52 +- .../include/common/mavlink_msg_param_set.h | 52 +- .../include/common/mavlink_msg_param_value.h | 50 +- .../mavlink/include/common/mavlink_msg_ping.h | 30 +- .../mavlink_msg_position_controller_output.h | 29 +- .../common/mavlink_msg_position_target.h | 28 +- .../include/common/mavlink_msg_raw_imu.h | 34 +- .../include/common/mavlink_msg_raw_pressure.h | 29 +- .../common/mavlink_msg_rc_channels_override.h | 286 ++++ .../common/mavlink_msg_rc_channels_raw.h | 33 +- .../common/mavlink_msg_rc_channels_scaled.h | 33 +- .../common/mavlink_msg_request_data_stream.h | 31 +- .../common/mavlink_msg_safety_allowed_area.h | 33 +- .../mavlink_msg_safety_set_allowed_area.h | 39 +- .../include/common/mavlink_msg_scaled_imu.h | 34 +- .../common/mavlink_msg_scaled_pressure.h | 28 +- .../common/mavlink_msg_servo_output_raw.h | 32 +- .../include/common/mavlink_msg_set_altitude.h | 28 +- .../include/common/mavlink_msg_set_mode.h | 26 +- .../include/common/mavlink_msg_set_nav_mode.h | 26 +- .../common/mavlink_msg_set_roll_pitch_yaw.h | 196 +++ .../mavlink_msg_set_roll_pitch_yaw_speed.h | 196 +++ .../common/mavlink_msg_state_correction.h | 33 +- .../include/common/mavlink_msg_statustext.h | 42 +- .../include/common/mavlink_msg_sys_status.h | 37 +- .../include/common/mavlink_msg_system_time.h | 25 +- .../common/mavlink_msg_system_time_utc.h | 26 +- .../include/common/mavlink_msg_vfr_hud.h | 34 +- .../include/common/mavlink_msg_waypoint.h | 52 +- .../include/common/mavlink_msg_waypoint_ack.h | 27 +- .../common/mavlink_msg_waypoint_clear_all.h | 26 +- .../common/mavlink_msg_waypoint_count.h | 29 +- .../common/mavlink_msg_waypoint_current.h | 25 +- .../common/mavlink_msg_waypoint_reached.h | 25 +- .../common/mavlink_msg_waypoint_request.h | 29 +- .../mavlink_msg_waypoint_request_list.h | 26 +- .../common/mavlink_msg_waypoint_set_current.h | 29 +- thirdParty/mavlink/include/mavlink_checksum.h | 156 +++ thirdParty/mavlink/include/mavlink_data.h | 21 + thirdParty/mavlink/include/mavlink_options.h | 102 ++ thirdParty/mavlink/include/mavlink_protocol.h | 951 +++++++++++++ thirdParty/mavlink/include/mavlink_types.h | 327 +++-- thirdParty/mavlink/include/minimal/mavlink.h | 7 +- .../include/minimal/mavlink_msg_heartbeat.h | 27 +- thirdParty/mavlink/include/minimal/minimal.h | 9 +- thirdParty/mavlink/include/pixhawk/mavlink.h | 7 +- .../pixhawk/mavlink_msg_attitude_control.h | 35 +- .../include/pixhawk/mavlink_msg_aux_status.h | 30 +- .../pixhawk/mavlink_msg_brief_feature.h | 52 +- .../mavlink_msg_data_transmission_handshake.h | 31 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 26 +- .../pixhawk/mavlink_msg_image_available.h | 57 +- .../mavlink_msg_image_trigger_control.h | 25 +- .../pixhawk/mavlink_msg_image_triggered.h | 36 +- .../include/pixhawk/mavlink_msg_marker.h | 33 +- .../pixhawk/mavlink_msg_pattern_detected.h | 46 +- .../pixhawk/mavlink_msg_point_of_interest.h | 62 +- ...mavlink_msg_point_of_interest_connection.h | 65 +- .../mavlink_msg_position_control_offset_set.h | 34 +- .../mavlink_msg_position_control_setpoint.h | 31 +- ...avlink_msg_position_control_setpoint_set.h | 37 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 33 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 36 +- .../mavlink_msg_vicon_position_estimate.h | 31 +- .../mavlink_msg_vision_position_estimate.h | 31 +- .../mavlink_msg_vision_speed_estimate.h | 28 +- .../pixhawk/mavlink_msg_watchdog_command.h | 30 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 26 +- .../mavlink_msg_watchdog_process_info.h | 57 +- .../mavlink_msg_watchdog_process_status.h | 34 +- thirdParty/mavlink/include/pixhawk/pixhawk.h | 9 +- thirdParty/mavlink/include/slugs/mavlink.h | 7 +- .../include/slugs/mavlink_msg_air_data.h | 27 +- .../include/slugs/mavlink_msg_cpu_load.h | 29 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 28 +- .../include/slugs/mavlink_msg_data_log.h | 30 +- .../include/slugs/mavlink_msg_diagnostic.h | 30 +- .../include/slugs/mavlink_msg_gps_date_time.h | 31 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 30 +- .../include/slugs/mavlink_msg_sensor_bias.h | 30 +- .../include/slugs/mavlink_msg_slugs_action.h | 29 +- .../slugs/mavlink_msg_slugs_navigation.h | 33 +- thirdParty/mavlink/include/slugs/slugs.h | 9 +- thirdParty/mavlink/include/ualberta/mavlink.h | 7 +- .../ualberta/mavlink_msg_nav_filter_bias.h | 31 +- .../ualberta/mavlink_msg_radio_calibration.h | 30 +- .../mavlink_msg_ualberta_sys_status.h | 27 +- .../mavlink/include/ualberta/ualberta.h | 9 +- .../message_definitions/ardupilotmega.xml | 6 +- .../mavlink/message_definitions/common.xml | 71 +- .../mavlink/message_definitions/minimal.xml | 20 +- .../mavlink/message_definitions/pixhawk.xml | 493 ++++--- .../mavlink/message_definitions/slugs.xml | 220 +-- .../mavlink/message_definitions/ualberta.xml | 102 +- .../mavlink/missionlib/testing/.gitignore | 1 + thirdParty/mavlink/missionlib/testing/main.c | 1221 +++++++++++++++++ thirdParty/mavlink/missionlib/testing/udp.c | 864 +++++++++++- thirdParty/mavlink/missionlib/waypoints.c | 880 ++++++++++++ thirdParty/mavlink/missionlib/waypoints.h | 91 ++ 135 files changed, 6671 insertions(+), 3703 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_full_state.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h create mode 100755 thirdParty/mavlink/include/mavlink_checksum.h create mode 100755 thirdParty/mavlink/include/mavlink_data.h create mode 100755 thirdParty/mavlink/include/mavlink_options.h create mode 100755 thirdParty/mavlink/include/mavlink_protocol.h mode change 100644 => 100755 thirdParty/mavlink/include/mavlink_types.h create mode 100644 thirdParty/mavlink/missionlib/testing/main.c create mode 100644 thirdParty/mavlink/missionlib/waypoints.c create mode 100644 thirdParty/mavlink/missionlib/waypoints.h diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc index 4b78f9fc67..90568e469a 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc @@ -97,11 +97,60 @@ void MAVLinkXMLParserV10::crcInit(uint16_t* crcAccum) } +const struct { + const char *prefix; + unsigned length; +} length_map[] = { + { "array", 1 }, + { "char", 1 }, + { "uint8", 1 }, + { "int8", 1 }, + { "uint16", 2 }, + { "int16", 2 }, + { "uint32", 4 }, + { "int32", 4 }, + { "uint64", 8 }, + { "int64", 8 }, + { "float", 4 }, + { "double", 8 }, +}; + +unsigned itemLength( const QString &s1 ) +{ + unsigned el1, i1, i2; + QString Ss1 = s1; + + Ss1 = Ss1.replace("_"," "); + Ss1 = Ss1.simplified(); + Ss1 = Ss1.section(" ",0,0); + + el1 = i1 = 0; + i2 = sizeof(length_map)/sizeof(length_map[0]); + + do { + if (Ss1.startsWith(length_map[i1].prefix)) + el1 = length_map[i1].length; + else ; + i1++; + } while ( (el1 == 0) && (i1 < i2)); + return el1; +} + +bool structSort(const QString &s1, const QString &s2) +{ + unsigned el1, el2; + + el1 = itemLength( s1 ); + el2 = itemLength( s2 ); + return el2 < el1; +} + /** * Generate C-code (C-89 compliant) out of the XML protocol specs. */ bool MAVLinkXMLParserV10::generate() { + uint16_t crc_key = X25_INIT_CRC; // Process result bool success = true; @@ -146,7 +195,15 @@ bool MAVLinkXMLParserV10::generate() QDir dir(outputDirName + "/" + messagesDirName); int mavlinkVersion = 0; - + static unsigned message_lengths[256]; + static unsigned message_key[256]; + static int highest_message_id; + static int recursion_level; + + if (recursion_level == 0) { + highest_message_id = 0; + memset(message_lengths, 0, sizeof(message_lengths)); + } // Start main header @@ -457,6 +514,7 @@ bool MAVLinkXMLParserV10::generate() QString sendArguments; QString commentLines; int calculatedLength = 0; + unsigned message_length = 0; // Get the message fields @@ -506,17 +564,17 @@ bool MAVLinkXMLParserV10::generate() QStringList itemList; // Swap field in C structure itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); cStructLines = itemList.join("\n") + "\n"; // Swap line in message_xx_pack function itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); packLines = itemList.join("\n") + "\n"; // Swap line in decode function for this type itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); decodeLines = itemList.join("\n") + "\n"; } } @@ -541,7 +599,7 @@ bool MAVLinkXMLParserV10::generate() // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); // Add decode function for this type - decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); + decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); if (fieldOffset != "") @@ -661,17 +719,17 @@ bool MAVLinkXMLParserV10::generate() QStringList itemList; // Swap field in C structure itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); cStructLines = itemList.join("\n") + "\n"; // Swap line in message_xx_pack function itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); packLines = itemList.join("\n") + "\n"; // Swap line in decode function for this type itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); decodeLines = itemList.join("\n") + "\n"; } if (fieldType.contains("char")) @@ -705,6 +763,26 @@ bool MAVLinkXMLParserV10::generate() } } + // message length calculation + unsigned element_multiplier = 1; + unsigned element_length = 0; + + if (fieldType.contains("[")) { + element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); + } + for (unsigned i=0; iERROR: Unable to calculate length for %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), fieldType)); + return false; + } + message_length += element_length; + // // QString unpackingCode; @@ -791,26 +869,71 @@ bool MAVLinkXMLParserV10::generate() f = f.nextSibling(); } + if (messageId > highest_message_id) { + highest_message_id = messageId; + } + message_lengths[messageId] = message_length; + + + // Sort fields to ensure 16bit-boundary aligned data + QStringList fieldList; + // Stable sort fields in C structure + fieldList = cStructLines.split("\n", QString::SkipEmptyParts); + if (fieldList.size() > 1) + { + qStableSort(fieldList.begin(), fieldList.end(), structSort); + } else ; + + // struct now sorted, do crc calc for each field + QString fieldCRCstring; + QByteArray fieldText; + crc_key = X25_INIT_CRC; + + for (int i =0; i < fieldList.size(); i++) + { + fieldCRCstring = fieldList.at(i).simplified(); + fieldCRCstring = fieldCRCstring.section(" ",0,1); // note: this has one space bewteen type and name + fieldText = fieldCRCstring.toAscii(); + for (int i = 0; i < fieldText.size(); ++i) + { + crcAccumulate((uint8_t) fieldText.at(i), &crc_key); + } + } + + // generate the key byte value + QString stringCRC; + message_key[messageId] = (crc_key&0xff)^((crc_key>>8)&0xff); + stringCRC = stringCRC.number( message_key[messageId], 16); + + // create structure + cStructLines = fieldList.join("\n") + "\n"; + + + // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); cStruct = cStruct.arg(cStructName, cStructLines ); lcmStructDefs.append("\n").append(cStruct).append("\n"); pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines); encode = encode.arg(messageName).arg(cStructName).arg(packArguments); - // decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); +// decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); decode = decode.arg(messageName).arg(cStructName); - // QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); - QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); - QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); - QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); - // compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); - compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; - QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); +// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); +// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); +// QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); +// QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); +// compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); +// compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; +// QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); + QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); QString compact2Send2("\thdr.sysid = mavlink_system.sysid;\n\thdr.compid = mavlink_system.compid;\n\thdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );\n"); QString compact2Send3("\n\tcrc_init(&checksum);\n\tchecksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);\n\tchecksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );\n\thdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\thdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);\n}\n\n#endif"); - compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3; - QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength)) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; + compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); +// QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; + QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); + + emit parseState(tr("Compiled message %1 \t(#%3) \tend near line %2, length %4, crc key 0x%5(%6)").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId), QString::number(message_lengths[messageId]), stringCRC.toUpper(), QString::number(message_key[messageId]))); } // Check if tag = message } // Check if e = NULL n = n.nextSibling(); @@ -854,6 +977,16 @@ bool MAVLinkXMLParserV10::generate() mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first); } + // CRC seeds + mainHeader += "\n\n// MESSAGE CRC KEYS\n\n"; + mainHeader += "#undef MAVLINK_MESSAGE_KEYS\n"; + mainHeader += "#define MAVLINK_MESSAGE_KEYS { "; + for (int i=0; i 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_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 }; @@ -274,6 +274,8 @@ enum MAV_CMD #include "./mavlink_msg_control_status.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" +#include "./mavlink_msg_set_roll_pitch_yaw.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed.h" #include "./mavlink_msg_attitude_controller_output.h" #include "./mavlink_msg_position_controller_output.h" #include "./mavlink_msg_nav_controller_output.h" @@ -281,7 +283,9 @@ enum MAV_CMD #include "./mavlink_msg_state_correction.h" #include "./mavlink_msg_set_altitude.h" #include "./mavlink_msg_request_data_stream.h" +#include "./mavlink_msg_full_state.h" #include "./mavlink_msg_manual_control.h" +#include "./mavlink_msg_rc_channels_override.h" #include "./mavlink_msg_global_position_int.h" #include "./mavlink_msg_vfr_hud.h" #include "./mavlink_msg_command.h" @@ -291,6 +295,13 @@ enum MAV_CMD #include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index ef3973fbbc..29d8958ff1 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:12 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "common.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action.h b/thirdParty/mavlink/include/common/mavlink_msg_action.h index 82ec1f50eb..636689544f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t comp * @param target_component The component executing the action * @param action The action id */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_action_t *p = (mavlink_action_t *)&msg.payload[0]; - - p->target = target; // uint8_t:The system executing the action - p->target_component = target_component; // uint8_t:The component executing the action - p->action = action; // uint8_t:The action id - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ACTION_LEN; - msg.msgid = MAVLINK_MSG_ID_ACTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h index bfb41a8194..b407815e7e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t * @param action The action id * @param result 0: Action DENIED, 1: Action executed */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg.payload[0]; - p->action = action; // uint8_t:The action id - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_ACTION_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 5d395fda6c..8c32cd48c3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg.payload[0]; - 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) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ATTITUDE_LEN; - msg.msgid = MAVLINK_MSG_ID_ATTITUDE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h index 081dd82d64..80d73978e6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t sys * @param yaw Attitude yaw: -128: -100%, 127: +100% * @param thrust Attitude thrust: -128: -100%, 127: +100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg.payload[0]; - - p->enabled = enabled; // uint8_t:1: enabled, 0: disabled - p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% - p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% - p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% - p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; - msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index f857ed91a7..36366e2dcb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -68,32 +68,9 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co * * @param key key */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg.payload[0]; - - memcpy(p->key, key, sizeof(p->key)); // char[32]:key - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; - msg.msgid = MAVLINK_MSG_ID_AUTH_KEY; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 00e13e2913..9051bb199a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon * * @param version The onboard software version */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_boot_t *p = (mavlink_boot_t *)&msg.payload[0]; - - p->version = version; // uint32_t:The onboard software version - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_BOOT_LEN; - msg.msgid = MAVLINK_MSG_ID_BOOT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { mavlink_header_t hdr; 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 fdc783caca..9b1a2e7958 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -86,35 +86,9 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg.payload[0]; - 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 "!?,.-" - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 b49883dd2d..8e93156755 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 @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index c22eb79833..7b8afd94b5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -6,14 +6,14 @@ typedef struct __mavlink_command_t { - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) float param1; ///< Parameter 1, as defined by MAV_CMD enum. float param2; ///< Parameter 2, as defined by MAV_CMD enum. float param3; ///< Parameter 3, as defined by MAV_CMD enum. float param4; ///< Parameter 4, as defined by MAV_CMD enum. + uint8_t target_system; ///< System which should execute the command + uint8_t target_component; ///< Component which should execute the command, 0 for all components + uint8_t command; ///< Command ID, as defined by MAV_CMD enum. + uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) } mavlink_command_t; @@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param3 Parameter 3, as defined by MAV_CMD enum. * @param param4 Parameter 4, as defined by MAV_CMD enum. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_command_t *p = (mavlink_command_t *)&msg.payload[0]; - - 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. - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_COMMAND_LEN; - msg.msgid = MAVLINK_MSG_ID_COMMAND; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index a82703d535..c7e4a4b0ff 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_COMMAND_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 00749b618e..8974f07d08 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -109,39 +109,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index 9924063356..419ef33e84 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -6,8 +6,8 @@ typedef struct __mavlink_debug_t { - uint8_t ind; ///< index of debug variable float value; ///< DEBUG value + uint8_t ind; ///< index of debug variable } mavlink_debug_t; @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_debug_t *p = (mavlink_debug_t *)&msg.payload[0]; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DEBUG_LEN; - msg.msgid = MAVLINK_MSG_ID_DEBUG; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index b31289ac84..53ef8bd11c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -6,11 +6,11 @@ typedef struct __mavlink_debug_vect_t { - char name[10]; ///< Name uint64_t usec; ///< Timestamp float x; ///< x float y; ///< y float z; ///< z + char name[10]; ///< Name } mavlink_debug_vect_t; #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 @@ -92,36 +92,9 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param y y * @param z z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; - msg.msgid = MAVLINK_MSG_ID_DEBUG_VECT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h new file mode 100644 index 0000000000..263d3e9678 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h @@ -0,0 +1,394 @@ +// MESSAGE FULL_STATE PACKING + +#define MAVLINK_MSG_ID_FULL_STATE 67 +#define MAVLINK_MSG_ID_FULL_STATE_LEN 56 +#define MAVLINK_MSG_67_LEN 56 + +typedef struct __mavlink_full_state_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + +} mavlink_full_state_t; + +/** + * @brief Pack a full_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FULL_STATE_LEN); +} + +/** + * @brief Pack a full_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FULL_STATE_LEN); +} + +/** + * @brief Encode a full_state struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param full_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_full_state_t* full_state) +{ + return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc); +} + +/** + * @brief Send a full_state message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_header_t hdr; + mavlink_full_state_t payload; + uint16_t checksum; + mavlink_full_state_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_FULL_STATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_FULL_STATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE FULL_STATE UNPACKING + +/** + * @brief Get field usec from full_state message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_full_state_get_usec(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (uint64_t)(p->usec); +} + +/** + * @brief Get field roll from full_state message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_full_state_get_roll(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from full_state message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_full_state_get_pitch(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from full_state message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_full_state_get_yaw(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Get field rollspeed from full_state message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_rollspeed(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->rollspeed); +} + +/** + * @brief Get field pitchspeed from full_state message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_pitchspeed(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->pitchspeed); +} + +/** + * @brief Get field yawspeed from full_state message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_yawspeed(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->yawspeed); +} + +/** + * @brief Get field lat from full_state message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_full_state_get_lat(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->lat); +} + +/** + * @brief Get field lon from full_state message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_full_state_get_lon(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->lon); +} + +/** + * @brief Get field alt from full_state message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_full_state_get_alt(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->alt); +} + +/** + * @brief Get field vx from full_state message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vx(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vx); +} + +/** + * @brief Get field vy from full_state message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vy(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vy); +} + +/** + * @brief Get field vz from full_state message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vz(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vz); +} + +/** + * @brief Get field xacc from full_state message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_xacc(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->xacc); +} + +/** + * @brief Get field yacc from full_state message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_yacc(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->yacc); +} + +/** + * @brief Get field zacc from full_state message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_zacc(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->zacc); +} + +/** + * @brief Decode a full_state message into a struct + * + * @param msg The message to decode + * @param full_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_full_state_decode(const mavlink_message_t* msg, mavlink_full_state_t* full_state) +{ + memcpy( full_state, msg->payload, sizeof(mavlink_full_state_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index bb5112eba5..26e2d8a133 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg.payload[0]; - 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) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; - msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 fb11a417ef..cc0f6e4428 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 */ -#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) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; - msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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) { mavlink_header_t hdr; 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 8834ee6ce9..831fc69689 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 @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index ecaeb8b849..6435fa3994 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -7,7 +7,6 @@ typedef struct __mavlink_gps_raw_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. float lat; ///< Latitude in degrees float lon; ///< Longitude in degrees float alt; ///< Altitude in meters @@ -15,6 +14,7 @@ typedef struct __mavlink_gps_raw_t float epv; ///< GPS VDOP float v; ///< GPS ground speed float hdg; ///< Compass heading in degrees, 0..360 degrees + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. } mavlink_gps_raw_t; @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees */ -#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) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_RAW_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_RAW; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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) { mavlink_header_t hdr; 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 9fc5360b1f..5ec33bd643 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -7,7 +7,6 @@ typedef struct __mavlink_gps_raw_int_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. int32_t lat; ///< Latitude in 1E7 degrees int32_t lon; ///< Longitude in 1E7 degrees int32_t alt; ///< Altitude in 1E3 meters (millimeters) @@ -15,6 +14,7 @@ typedef struct __mavlink_gps_raw_int_t float epv; ///< GPS VDOP float v; ///< GPS ground speed (m/s) 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. } mavlink_gps_raw_int_t; @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param v GPS ground speed (m/s) * @param hdg Compass heading in degrees, 0..360 degrees */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg.payload[0]; - 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) - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed (m/s) - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) { mavlink_header_t hdr; 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 f7aaf96da1..bccfdba02c 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 @@ -6,11 +6,11 @@ typedef struct __mavlink_gps_set_global_origin_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID int32_t latitude; ///< global position * 1E7 int32_t longitude; ///< global position * 1E7 int32_t altitude; ///< global position * 1000 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_gps_set_global_origin_t; @@ -91,36 +91,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index 484de2c012..a974432d6d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -102,37 +102,9 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. * @param satellite_snr Signal to noise ratio of satellite */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index fea5381e9c..7c686831ee 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -76,34 +76,9 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; - - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; - msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index 2fbe01f2ab..fb6648714a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; - msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 2f6ceb808c..8ea4ca458c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; - msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 9f34862b8e..d7aa0d5bc0 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 @@ -6,12 +6,12 @@ typedef struct __mavlink_local_position_setpoint_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< Desired yaw angle + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_local_position_setpoint_set_t; @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index d3aede95be..eeb05487af 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -6,11 +6,11 @@ typedef struct __mavlink_manual_control_t { - uint8_t target; ///< The system to be controlled float roll; ///< roll float pitch; ///< pitch float yaw; ///< yaw float thrust; ///< thrust + uint8_t target; ///< The system to be controlled uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 uint8_t pitch_manual; ///< pitch auto:0, manual:1 uint8_t yaw_manual; ///< yaw auto:0, manual:1 @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 6f4fb8830f..ce5c680ad2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -6,8 +6,8 @@ typedef struct __mavlink_named_value_float_t { - char name[10]; ///< Name of the debug variable float value; ///< Floating point value + char name[10]; ///< Name of the debug variable } mavlink_named_value_float_t; #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 @@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param name Name of the debug variable * @param value Floating point value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg.payload[0]; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; - msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 a793614f5e..1c75a5be16 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -6,8 +6,8 @@ typedef struct __mavlink_named_value_int_t { - char name[10]; ///< Name of the debug variable int32_t value; ///< Signed integer value + char name[10]; ///< Name of the debug variable } mavlink_named_value_int_t; #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 @@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param name Name of the debug variable * @param value Signed integer value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg.payload[0]; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; - msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 f19d85b0a1..ef878a3f7c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -8,12 +8,12 @@ typedef struct __mavlink_nav_controller_output_t { float nav_roll; ///< Current desired roll in degrees float nav_pitch; ///< Current desired pitch in degrees - int16_t nav_bearing; ///< Current desired heading in degrees - int16_t target_bearing; ///< Bearing to current waypoint/target in degrees - uint16_t wp_dist; ///< Distance to active waypoint in meters float alt_error; ///< Current altitude error in meters float aspd_error; ///< Current airspeed error in meters/second float xtrack_error; ///< Current crosstrack error on x-y plane in meters + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current waypoint/target in degrees + uint16_t wp_dist; ///< Distance to active waypoint in meters } mavlink_nav_controller_output_t; @@ -109,39 +109,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; - msg.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 d53cfe7d8b..74ed51442e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 a60179da97..07e3ae044e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_REQUEST_READ PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 -#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 19 -#define MAVLINK_MSG_20_LEN 19 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_20_LEN 20 typedef struct __mavlink_param_request_read_t { + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - char param_id[15]; ///< Onboard parameter id - int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier + uint8_t param_id[16]; ///< Onboard parameter id } mavlink_param_request_read_t; -#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15 +#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_request_read message @@ -26,14 +26,14 @@ 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 uint8_t* 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); @@ -51,14 +51,14 @@ 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 uint8_t* 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); @@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_id Onboard parameter id * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg.payload[0]; - 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[15]:Onboard parameter id - p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 uint8_t* param_id, int16_t param_index) { mavlink_header_t hdr; mavlink_param_request_read_t payload; @@ -124,7 +98,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier hdr.STX = MAVLINK_STX; @@ -176,7 +150,7 @@ 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, uint8_t* param_id) { mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index 476c980957..396a5307e6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 -#define MAVLINK_MSG_ID_PARAM_SET_LEN 21 -#define MAVLINK_MSG_23_LEN 21 +#define MAVLINK_MSG_ID_PARAM_SET_LEN 22 +#define MAVLINK_MSG_23_LEN 22 typedef struct __mavlink_param_set_t { + float param_value; ///< Onboard parameter value uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - char param_id[15]; ///< Onboard parameter id - float param_value; ///< Onboard parameter value + uint8_t param_id[16]; ///< Onboard parameter id } mavlink_param_set_t; -#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15 +#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_set message @@ -26,14 +26,14 @@ typedef struct __mavlink_param_set_t * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) { 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); @@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) { 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); @@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_id Onboard parameter id * @param param_value Onboard parameter value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg.payload[0]; - 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[15]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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) +#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 uint8_t* param_id, float param_value) { mavlink_header_t hdr; mavlink_param_set_t payload; @@ -124,7 +98,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value hdr.STX = MAVLINK_STX; @@ -176,7 +150,7 @@ 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, uint8_t* param_id) { mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 3834408454..636589cd07 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 -#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 23 -#define MAVLINK_MSG_22_LEN 23 +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 24 +#define MAVLINK_MSG_22_LEN 24 typedef struct __mavlink_param_value_t { - char param_id[15]; ///< Onboard parameter id float param_value; ///< Onboard parameter value uint16_t param_count; ///< Total number of onboard parameters uint16_t param_index; ///< Index of this onboard parameter + uint8_t param_id[16]; ///< Onboard parameter id } mavlink_param_value_t; -#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 15 +#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_value message @@ -26,12 +26,12 @@ 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, 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 uint8_t* param_id, float param_value, 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value p->param_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -51,12 +51,12 @@ 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, 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 uint8_t* param_id, float param_value, 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[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value p->param_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -86,43 +86,17 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_count Total number of onboard parameters * @param param_index Index of this onboard parameter */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg.payload[0]; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_VALUE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_header_t hdr; mavlink_param_value_t payload; uint16_t checksum; mavlink_param_value_t *p = &payload; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value p->param_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -154,7 +128,7 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch * * @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, uint8_t* param_id) { mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index 2e89aa2b0c..f22a195c79 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -6,10 +6,10 @@ typedef struct __mavlink_ping_t { + uint64_t time; ///< Unix timestamp in microseconds uint32_t seq; ///< PING sequence uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - uint64_t time; ///< Unix timestamp in microseconds } mavlink_ping_t; @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_ping_t *p = (mavlink_ping_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PING_LEN; - msg.msgid = MAVLINK_MSG_ID_PING; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h index 2935301ef4..951c293232 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t sys * @param z Position z: -128: -100%, 127: +100% * @param yaw Position yaw: -128: -100%, 127: +100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg.payload[0]; - - p->enabled = enabled; // uint8_t:1: enabled, 0: disabled - p->x = x; // int8_t:Position x: -128: -100%, 127: +100% - p->y = y; // int8_t:Position y: -128: -100%, 127: +100% - p->z = z; // int8_t:Position z: -128: -100%, 127: +100% - p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index 7d9c425a62..1a65ed0a40 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_TARGET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index a815b1fa03..aa0f306727 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -121,41 +121,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg.payload[0]; - - 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) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RAW_IMU_LEN; - msg.msgid = MAVLINK_MSG_ID_RAW_IMU; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index ccd6c59ac8..4d3a9ba258 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -91,36 +91,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg.payload[0]; - - 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) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; - msg.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h new file mode 100644 index 0000000000..b9dfe85e96 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -0,0 +1,286 @@ +// MESSAGE RC_CHANNELS_OVERRIDE PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_70_LEN 18 + +typedef struct __mavlink_rc_channels_override_t +{ + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_rc_channels_override_t; + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +} + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +} + +/** + * @brief Encode a rc_channels_override struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** + * @brief Send a rc_channels_override message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_header_t hdr; + mavlink_rc_channels_override_t payload; + uint16_t checksum; + mavlink_rc_channels_override_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING + +/** + * @brief Get field target_system from rc_channels_override message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from rc_channels_override message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field chan1_raw from rc_channels_override message + * + * @return RC channel 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan1_raw); +} + +/** + * @brief Get field chan2_raw from rc_channels_override message + * + * @return RC channel 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan2_raw); +} + +/** + * @brief Get field chan3_raw from rc_channels_override message + * + * @return RC channel 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan3_raw); +} + +/** + * @brief Get field chan4_raw from rc_channels_override message + * + * @return RC channel 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan4_raw); +} + +/** + * @brief Get field chan5_raw from rc_channels_override message + * + * @return RC channel 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan5_raw); +} + +/** + * @brief Get field chan6_raw from rc_channels_override message + * + * @return RC channel 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan6_raw); +} + +/** + * @brief Get field chan7_raw from rc_channels_override message + * + * @return RC channel 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan7_raw); +} + +/** + * @brief Get field chan8_raw from rc_channels_override message + * + * @return RC channel 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan8_raw); +} + +/** + * @brief Decode a rc_channels_override message into a struct + * + * @param msg The message to decode + * @param rc_channels_override C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override) +{ + memcpy( rc_channels_override, msg->payload, sizeof(mavlink_rc_channels_override_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h index 7f7351d652..56de4d83c8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg.payload[0]; - 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% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; - msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 16bf4791e3..fd2e57ad4c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg.payload[0]; - 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% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; - msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 4d48f09540..b779e267e8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -6,10 +6,10 @@ typedef struct __mavlink_request_data_stream_t { + uint16_t req_message_rate; ///< The requested interval between two messages of this type uint8_t target_system; ///< The target requested to send the message stream. uint8_t target_component; ///< The target requested to send the message stream. uint8_t req_stream_id; ///< The ID of the requested message type - uint16_t req_message_rate; ///< The requested interval between two messages of this type uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; @@ -91,36 +91,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:The target requested to send the message stream. - p->target_component = target_component; // uint8_t:The target requested to send the message stream. - p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; - msg.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 aaee3bf13a..a660e8bed5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -6,13 +6,13 @@ typedef struct __mavlink_safety_allowed_area_t { - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. float p1x; ///< x position 1 / Latitude 1 float p1y; ///< y position 1 / Longitude 1 float p1z; ///< z position 1 / Altitude 1 float p2x; ///< x position 2 / Latitude 2 float p2y; ///< y position 2 / Longitude 2 float p2z; ///< z position 2 / Altitude 2 + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. } mavlink_safety_allowed_area_t; @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; - msg.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 4427dbedd7..9e8ef1e44b 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 @@ -6,15 +6,15 @@ typedef struct __mavlink_safety_set_allowed_area_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. float p1x; ///< x position 1 / Latitude 1 float p1y; ///< y position 1 / Longitude 1 float p1z; ///< z position 1 / Altitude 1 float p2x; ///< x position 2 / Latitude 2 float p2y; ///< y position 2 / Longitude 2 float p2z; ///< z position 2 / Altitude 2 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. } mavlink_safety_set_allowed_area_t; @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; - msg.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 44fe8bdbdd..8f381f22c1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -121,41 +121,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg.payload[0]; - - 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) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; - msg.msgid = MAVLINK_MSG_ID_SCALED_IMU; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index bf653ad5a3..39d29440f2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg.payload[0]; - 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) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; - msg.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 f12eb29a0d..5aa3cc8d0d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -109,39 +109,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; - msg.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 3e32eee15f..9b50027f1d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -6,8 +6,8 @@ typedef struct __mavlink_set_altitude_t { - uint8_t target; ///< The system setting the altitude uint32_t mode; ///< The new altitude in meters + uint8_t target; ///< The system setting the altitude } mavlink_set_altitude_t; @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index a74108ee8e..ad796f1e0e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_MODE_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_MODE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 649fbee575..1147440097 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h new file mode 100644 index 0000000000..1fc909fc4b --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h @@ -0,0 +1,196 @@ +// MESSAGE SET_ROLL_PITCH_YAW PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN 14 +#define MAVLINK_MSG_55_LEN 14 + +typedef struct __mavlink_set_roll_pitch_yaw_t +{ + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_set_roll_pitch_yaw_t; + +/** + * @brief Pack a set_roll_pitch_yaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) +{ + return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw); +} + +/** + * @brief Send a set_roll_pitch_yaw message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_set_roll_pitch_yaw_t payload; + uint16_t checksum; + mavlink_set_roll_pitch_yaw_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll from set_roll_pitch_yaw message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Decode a set_roll_pitch_yaw message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) +{ + memcpy( set_roll_pitch_yaw, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h new file mode 100644 index 0000000000..bb5858356f --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h @@ -0,0 +1,196 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN 14 +#define MAVLINK_MSG_56_LEN 14 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_t +{ + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_set_roll_pitch_yaw_speed_t; + +/** + * @brief Pack a set_roll_pitch_yaw_speed message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed); +} + +/** + * @brief Send a set_roll_pitch_yaw_speed message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_header_t hdr; + mavlink_set_roll_pitch_yaw_speed_t payload; + uint16_t checksum; + mavlink_set_roll_pitch_yaw_speed_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) +{ + memcpy( set_roll_pitch_yaw_speed, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index 6ca16a5347..f101513eb8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; - msg.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index 485bbf1ccc..bc9c77b8fa 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -7,7 +7,7 @@ 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 + int8_t text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 @@ -22,13 +22,13 @@ 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 int8_t* 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 + memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -43,13 +43,13 @@ 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 int8_t* 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 + memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -74,34 +74,10 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param severity Severity of status, 0 = info message, 255 = critical fault * @param text Status text message, without null termination character */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_statustext_t *p = (mavlink_statustext_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; - msg.msgid = MAVLINK_MSG_ID_STATUSTEXT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 int8_t* text) { mavlink_header_t hdr; mavlink_statustext_t payload; @@ -109,7 +85,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s mavlink_statustext_t *p = &payload; p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; @@ -149,7 +125,7 @@ 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, int8_t* text) { mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index a340cfaa8c..1ca8cec2ec 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -6,13 +6,13 @@ typedef struct __mavlink_sys_status_t { - uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - uint8_t status; ///< System status flag, see MAV_STATUS ENUM uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) + uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + uint8_t status; ///< System status flag, see MAV_STATUS ENUM } mavlink_sys_status_t; @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg.payload[0]; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_SYS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index 2213495fc0..c2328a68fc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t * * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_system_time_t *p = (mavlink_system_time_t *)&msg.payload[0]; - - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; - msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { mavlink_header_t hdr; 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 04d124f2b5..414b4984f0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg.payload[0]; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; - msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index ee92a1e34c..0b757cef66 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -8,10 +8,10 @@ typedef struct __mavlink_vfr_hud_t { float airspeed; ///< Current airspeed in m/s float groundspeed; ///< Current ground speed in m/s - int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 float alt; ///< Current altitude (MSL), in meters float climb; ///< Current climb rate in meters/second + int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 } mavlink_vfr_hud_t; @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VFR_HUD_LEN; - msg.msgid = MAVLINK_MSG_ID_VFR_HUD; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index 07f7a08bd1..a29a26b0ec 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -6,13 +6,6 @@ typedef struct __mavlink_waypoint_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence - uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. @@ -20,6 +13,13 @@ typedef struct __mavlink_waypoint_t float x; ///< PARAM5 / local: x position, global: latitude float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp } mavlink_waypoint_t; @@ -145,45 +145,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 8bdd5ae15a..8a5d52ed38 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 efccf36008..b96a80696a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index a446197215..8856c3c445 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -6,9 +6,9 @@ typedef struct __mavlink_waypoint_count_t { + uint16_t count; ///< Number of Waypoints in the Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t count; ///< Number of Waypoints in the Sequence } mavlink_waypoint_count_t; @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 59b2f6b3a7..9c75639c79 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui * * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg.payload[0]; - - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 17404e658b..5e1f144e8b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui * * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg.payload[0]; - - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index c33edce8e2..9f3ccdbe31 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -6,9 +6,9 @@ typedef struct __mavlink_waypoint_request_t { + uint16_t seq; ///< Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence } mavlink_waypoint_request_t; @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 209b6140eb..34d8276d07 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 c35fe21075..829f29982b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -6,9 +6,9 @@ typedef struct __mavlink_waypoint_set_current_t { + uint16_t seq; ///< Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence } mavlink_waypoint_set_current_t; @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/mavlink_checksum.h b/thirdParty/mavlink/include/mavlink_checksum.h new file mode 100755 index 0000000000..0f42251741 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_checksum.h @@ -0,0 +1,156 @@ +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_data.h b/thirdParty/mavlink/include/mavlink_data.h new file mode 100755 index 0000000000..ce51e2b93a --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_data.h @@ -0,0 +1,21 @@ +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + +#endif \ No newline at end of file diff --git a/thirdParty/mavlink/include/mavlink_options.h b/thirdParty/mavlink/include/mavlink_options.h new file mode 100755 index 0000000000..646ebed02a --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_options.h @@ -0,0 +1,102 @@ +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STAIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h new file mode 100755 index 0000000000..62163d5983 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -0,0 +1,951 @@ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + union checksum_ ck; + crc_init(&(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + union checksum_ ck; + ck.c[0] = msg->ck_a; + ck.c[1] = msg->ck_b; + crc_accumulate(c, &(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STAIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_lengths[rxmsg->msgid] ) ); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#define FILE_FINISHED + +#ifndef FILE_FINISHED +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ + +#define MAVLINK_PACKET_START_CANDIDATES 50 +/* +static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; + #else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; + #endif + + // Set a packet start candidate index if sign is start sign + if (c == MAVLINK_STX) + { + m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; + } + + // Parse normally, if a CRC mismatch occurs retry with the next packet index +} +//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +//#else +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +//#endif +//// Initializes only once, values keep unchanged after first initialization +// mavlink_parse_state_initialize(&m_mavlink_status[chan]); +// +//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message +//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status +//int bufferIndex = 0; +// +//status->msg_received = 0; +// +//switch (status->parse_state) +//{ +//case MAVLINK_PARSE_STATE_UNINIT: +//case MAVLINK_PARSE_STATE_IDLE: +// if (c == MAVLINK_STX) +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; +// mavlink_start_checksum(rxmsg); +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_STX: +// if (status->msg_received) +// { +// status->buffer_overrun++; +// status->parse_error++; +// status->msg_received = 0; +// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// } +// else +// { +// // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 +// rxmsg->len = c; +// status->packet_idx = 0; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_LENGTH: +// rxmsg->seq = c; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; +// break; +// +//case MAVLINK_PARSE_STATE_GOT_SEQ: +// rxmsg->sysid = c; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; +// break; +// +//case MAVLINK_PARSE_STATE_GOT_SYSID: +// rxmsg->compid = c; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; +// break; +// +//case MAVLINK_PARSE_STATE_GOT_COMPID: +// rxmsg->msgid = c; +// mavlink_update_checksum(rxmsg, c); +// if (rxmsg->len == 0) +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; +// } +// else +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_MSGID: +// rxmsg->payload[status->packet_idx++] = c; +// mavlink_update_checksum(rxmsg, c); +// if (status->packet_idx == rxmsg->len) +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_PAYLOAD: +// if (c != rxmsg->ck_a) +// { +// // Check first checksum byte +// status->parse_error++; +// status->msg_received = 0; +// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// } +// else +// { +// 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; +// } +// 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_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; +} + +r_mavlink_status->current_seq = status->current_seq+1; +r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; +r_mavlink_status->packet_rx_drop_count = status->parse_error; +return status->msg_received; +} + */ + + +typedef union __generic_16bit +{ + uint8_t b[2]; + int16_t s; +} generic_16bit; + +typedef union __generic_32bit +{ + uint8_t b[4]; + float f; + int32_t i; + int16_t s; +} generic_32bit; + +typedef union __generic_64bit +{ + uint8_t b[8]; + int64_t ll; ///< Long long (64 bit) +} generic_64bit; + +/** + * @brief Place an unsigned byte into the buffer + * + * @param b the byte to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) +{ + *(buffer + bindex) = b; + return sizeof(b); +} + +/** + * @brief Place a signed byte into the buffer + * + * @param b the byte to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) +{ + *(buffer + bindex) = (uint8_t)b; + return sizeof(b); +} + +/** + * @brief Place two unsigned bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>8)&0xff; + buffer[bindex+1] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place two signed bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) +{ + return put_uint16_t_by_index(b, bindex, buffer); +} + +/** + * @brief Place four unsigned bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>24)&0xff; + buffer[bindex+1] = (b>>16)&0xff; + buffer[bindex+2] = (b>>8)&0xff; + buffer[bindex+3] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place four signed bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>24)&0xff; + buffer[bindex+1] = (b>>16)&0xff; + buffer[bindex+2] = (b>>8)&0xff; + buffer[bindex+3] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place four unsigned bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>56)&0xff; + buffer[bindex+1] = (b>>48)&0xff; + buffer[bindex+2] = (b>>40)&0xff; + buffer[bindex+3] = (b>>32)&0xff; + buffer[bindex+4] = (b>>24)&0xff; + buffer[bindex+5] = (b>>16)&0xff; + buffer[bindex+6] = (b>>8)&0xff; + buffer[bindex+7] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place four signed bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) +{ + return put_uint64_t_by_index(b, bindex, buffer); +} + +/** + * @brief Place a float into the buffer + * + * @param b the float to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) +{ + generic_32bit g; + g.f = b; + return put_int32_t_by_index(g.i, bindex, buffer); +} + +/** + * @brief Place an array into the buffer + * + * @param b the array to add + * @param length size of the array (for strings: length WITH '\0' char) + * @param bindex the position in the packet + * @param buffer packet buffer + * @return new position of the last used byte in the buffer + */ +static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) +{ + memcpy(buffer+bindex, b, length); + return length; +} + +/** + * @brief Place a string into the buffer + * + * @param b the string to add + * @param maxlength size of the array (for strings: length WITHOUT '\0' char) + * @param bindex the position in the packet + * @param buffer packet buffer + * @return new position of the last used byte in the buffer + */ +static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) +{ + uint16_t length = 0; + // Copy string into buffer, ensuring not to exceed the buffer size + int i; + for (i = 1; i < maxlength; i++) + { + length++; + // String characters + if (i < (maxlength - 1)) + { + buffer[bindex+i] = b[i]; + // Stop at null character + if (b[i] == '\0') + { + break; + } + } + // Enforce null termination at end of buffer + else if (i == (maxlength - 1)) + { + buffer[i] = '\0'; + } + } + // Write length into first field + put_uint8_t_by_index(length, bindex, buffer); + return length; +} + +/** + * @brief Put a bitfield of length 1-32 bit into the buffer + * + * @param b the value to add, will be encoded in the bitfield + * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. + * @param packet_index the position in the packet (the index of the first byte to use) + * @param bit_index the position in the byte (the index of the first bit to use) + * @param buffer packet buffer to write into + * @return new position of the last used byte in the buffer + */ +static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) +{ + uint16_t bits_remain = bits; + // Transform number into network order + generic_32bit bin; + generic_32bit bout; + uint8_t i_bit_index, i_byte_index, curr_bits_n; + bin.i = b; + bout.b[0] = bin.b[3]; + bout.b[1] = bin.b[2]; + bout.b[2] = bin.b[1]; + bout.b[3] = bin.b[0]; + + // buffer in + // 01100000 01000000 00000000 11110001 + // buffer out + // 11110001 00000000 01000000 01100000 + + // Existing partly filled byte (four free slots) + // 0111xxxx + + // Mask n free bits + // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 + // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 + + // Shift n bits into the right position + // out = in >> n; + + // Mask and shift bytes + i_bit_index = bit_index; + i_byte_index = packet_index; + if (bit_index > 0) + { + // If bits were available at start, they were available + // in the byte before the current index + i_byte_index--; + } + + // While bits have not been packed yet + while (bits_remain > 0) + { + // Bits still have to be packed + // there can be more than 8 bits, so + // we might have to pack them into more than one byte + + // First pack everything we can into the current 'open' byte + //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 + //FIXME + if (bits_remain <= (8 - i_bit_index)) + { + // Enough space + curr_bits_n = bits_remain; + } + else + { + curr_bits_n = (8 - i_bit_index); + } + + // Pack these n bits into the current byte + // Mask out whatever was at that position with ones (xxx11111) + buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); + // Put content to this position, by masking out the non-used part + buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); + + // Increment the bit index + i_bit_index += curr_bits_n; + + // Now proceed to the next byte, if necessary + bits_remain -= curr_bits_n; + if (bits_remain > 0) + { + // Offer another 8 bits / one byte + i_byte_index++; + i_bit_index = 0; + } + } + + *r_bit_index = i_bit_index; + // If a partly filled byte is present, mark this as consumed + if (i_bit_index != 7) i_byte_index++; + return i_byte_index - packet_index; +} +*/ +#endif /* FILE_FINISHED */ + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h old mode 100644 new mode 100755 index 199fc56f49..3124ba621d --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -1,87 +1,240 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include "inttypes.h" - -#define MAVLINK_STX 0xD5 ///< Packet start sign -#define MAVLINK_STX_LEN 1 ///< Length of start sign -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length -//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN - -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message { - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU -} mavlink_message_t; - -typedef struct __mavlink_header { - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload -} mavlink_header_t; - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_NB, - MAVLINK_COMM_NB_HIGH = 16 -} mavlink_channel_t; - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status { - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#endif /* MAVLINK_TYPES_H_ */ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +enum MAV_CLASS +{ + MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything + MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch + MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu + MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com + MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands + MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set + MAV_CLASS_NB ///< Number of autopilot classes +}; + +enum MAV_ACTION +{ + MAV_ACTION_HOLD = 0, + MAV_ACTION_MOTORS_START = 1, + MAV_ACTION_LAUNCH = 2, + MAV_ACTION_RETURN = 3, + MAV_ACTION_EMCY_LAND = 4, + MAV_ACTION_EMCY_KILL = 5, + MAV_ACTION_CONFIRM_KILL = 6, + MAV_ACTION_CONTINUE = 7, + MAV_ACTION_MOTORS_STOP = 8, + MAV_ACTION_HALT = 9, + MAV_ACTION_SHUTDOWN = 10, + MAV_ACTION_REBOOT = 11, + MAV_ACTION_SET_MANUAL = 12, + MAV_ACTION_SET_AUTO = 13, + MAV_ACTION_STORAGE_READ = 14, + MAV_ACTION_STORAGE_WRITE = 15, + MAV_ACTION_CALIBRATE_RC = 16, + MAV_ACTION_CALIBRATE_GYRO = 17, + MAV_ACTION_CALIBRATE_MAG = 18, + MAV_ACTION_CALIBRATE_ACC = 19, + MAV_ACTION_CALIBRATE_PRESSURE = 20, + MAV_ACTION_REC_START = 21, + MAV_ACTION_REC_PAUSE = 22, + MAV_ACTION_REC_STOP = 23, + MAV_ACTION_TAKEOFF = 24, + MAV_ACTION_NAVIGATE = 25, + MAV_ACTION_LAND = 26, + MAV_ACTION_LOITER = 27, + MAV_ACTION_SET_ORIGIN = 28, + MAV_ACTION_RELAY_ON = 29, + MAV_ACTION_RELAY_OFF = 30, + MAV_ACTION_GET_IMAGE = 31, + MAV_ACTION_VIDEO_START = 32, + MAV_ACTION_VIDEO_STOP = 33, + MAV_ACTION_RESET_MAP = 34, + MAV_ACTION_RESET_PLAN = 35, + MAV_ACTION_DELAY_BEFORE_COMMAND = 36, + MAV_ACTION_ASCEND_AT_RATE = 37, + MAV_ACTION_CHANGE_MODE = 38, + MAV_ACTION_LOITER_MAX_TURNS = 39, + MAV_ACTION_LOITER_MAX_TIME = 40, + MAV_ACTION_START_HILSIM = 41, + MAV_ACTION_STOP_HILSIM = 42, + MAV_ACTION_NB ///< Number of MAV actions +}; + +enum MAV_MODE +{ + MAV_MODE_UNINIT = 0, ///< System is in undefined state + MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe + MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control + MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint + MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation + MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use + MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use + MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use + MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive + MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back +}; + +enum MAV_STATE +{ + MAV_STATE_UNINIT = 0, + MAV_STATE_BOOT, + MAV_STATE_CALIBRATING, + MAV_STATE_STANDBY, + MAV_STATE_ACTIVE, + MAV_STATE_CRITICAL, + MAV_STATE_EMERGENCY, + MAV_STATE_POWEROFF +}; + +enum MAV_NAV +{ + MAV_NAV_GROUNDED = 0, + MAV_NAV_LIFTOFF, + MAV_NAV_HOLD, + MAV_NAV_WAYPOINT, + MAV_NAV_VECTOR, + MAV_NAV_RETURNING, + MAV_NAV_LANDING, + MAV_NAV_LOST, + MAV_NAV_LOITER +}; + +enum MAV_TYPE +{ + MAV_GENERIC = 0, + MAV_FIXED_WING = 1, + MAV_QUADROTOR = 2, + MAV_COAXIAL = 3, + MAV_HELICOPTER = 4, + MAV_GROUND = 5, + OCU = 6 +}; + +enum MAV_AUTOPILOT_TYPE +{ + MAV_AUTOPILOT_GENERIC = 0, + MAV_AUTOPILOT_PIXHAWK = 1, + MAV_AUTOPILOT_SLUGS = 2, + MAV_AUTOPILOT_ARDUPILOTMEGA = 3 +}; + +enum MAV_COMPONENT +{ + MAV_COMP_ID_GPS, + MAV_COMP_ID_WAYPOINTPLANNER, + MAV_COMP_ID_BLOBTRACKER, + MAV_COMP_ID_PATHPLANNER, + MAV_COMP_ID_AIRSLAM, + MAV_COMP_ID_MAPPER, + MAV_COMP_ID_CAMERA, + MAV_COMP_ID_IMU = 200, + MAV_COMP_ID_UDP_BRIDGE = 240, + MAV_COMP_ID_UART_BRIDGE = 241, + MAV_COMP_ID_SYSTEM_CONTROL = 250 +}; + +enum MAV_FRAME +{ + MAV_FRAME_GLOBAL = 0, + MAV_FRAME_LOCAL = 1, + MAV_FRAME_MISSION = 2, + MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 +}; + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system { + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode +} mavlink_system_t; + +typedef struct __mavlink_message { + union { + uint16_t ck; ///< Checksum high byte + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header { + union { + uint16_t ck; ///< Checksum high byte + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload +} mavlink_header_t; + +typedef enum { + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 + } mavlink_channel_t; + +typedef enum { + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status { + uint8_t ck_a; ///< Checksum byte 1 + uint8_t ck_b; ///< Checksum byte 2 + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 7cfe0d9d02..c8cd1940e2 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "minimal.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index fea5381e9c..7c686831ee 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -76,34 +76,9 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; - - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; - msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index 47a266ccb9..c3d7e2a1bf 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -32,6 +32,13 @@ extern "C" { // MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index 963c3697ab..c59c58b83e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "pixhawk.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index 25ed694100..fd8503d974 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -6,11 +6,11 @@ typedef struct __mavlink_attitude_control_t { - uint8_t target; ///< The system to be controlled float roll; ///< roll float pitch; ///< pitch float yaw; ///< yaw float thrust; ///< thrust + uint8_t target; ///< The system to be controlled uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 uint8_t pitch_manual; ///< pitch auto:0, manual:1 uint8_t yaw_manual; ///< yaw auto:0, manual:1 @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index 9cb8641e82..2fb3314c93 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param spi1_err_count Number of I2C errors since startup * @param uart_total_err_count Number of I2C errors since startup */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg.payload[0]; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_AUX_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index 96814fcabc..4e6f6b83bc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -9,11 +9,11 @@ typedef struct __mavlink_brief_feature_t float x; ///< x position in m float y; ///< y position in m float z; ///< z position in m - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + float response; ///< Harris operator response at this location uint16_t size; ///< Size in pixels uint16_t orientation; ///< Orientation - char descriptor[32]; ///< Descriptor - float response; ///< Harris operator response at this location + uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + uint8_t descriptor[32]; ///< Descriptor } mavlink_brief_feature_t; #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 @@ -34,7 +34,7 @@ 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 char* 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; @@ -45,7 +45,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t 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)); // char[32]:Descriptor + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor p->response = response; // float:Harris operator response at this location return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); @@ -67,7 +67,7 @@ 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 char* 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) { mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; @@ -78,7 +78,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui 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)); // char[32]:Descriptor + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor p->response = response; // float:Harris operator response at this location return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); @@ -110,40 +110,10 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param descriptor Descriptor * @param response Harris operator response at this location */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -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 char* descriptor, float response) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg.payload[0]; - - 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)); // char[32]:Descriptor - p->response = response; // float:Harris operator response at this location - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; - msg.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 char* 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; @@ -156,7 +126,7 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float 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)); // char[32]:Descriptor + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor p->response = response; // float:Harris operator response at this location hdr.STX = MAVLINK_STX; @@ -252,7 +222,7 @@ 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, char* 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]; 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 22116a54c8..54d2070a98 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -6,8 +6,8 @@ typedef struct __mavlink_data_transmission_handshake_t { - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) uint32_t size; ///< total data size in bytes (set on ACK only) + uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) uint8_t packets; ///< number of packets beeing sent (set on ACK only) uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) uint8_t jpg_quality; ///< JPEG quality out of [1,100] @@ -91,36 +91,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg.payload[0]; - - 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] - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; - msg.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index cd052fcb49..a5354fdded 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; - msg.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index 49869a01ec..0b4c9c8eff 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -7,15 +7,10 @@ typedef struct __mavlink_image_available_t { uint64_t cam_id; ///< Camera id - uint8_t cam_no; ///< Camera # (starts with 0) uint64_t timestamp; ///< Timestamp uint64_t valid_until; ///< Until which timestamp this buffer will stay valid uint32_t img_seq; ///< The image sequence number uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t channels; ///< Image channels uint32_t key; ///< Shared memory area key uint32_t exposure; ///< Exposure time, in microseconds float gain; ///< Camera gain @@ -29,6 +24,11 @@ typedef struct __mavlink_image_available_t float ground_x; ///< Ground truth X float ground_y; ///< Ground truth Y float ground_z; ///< Ground truth Z + uint16_t width; ///< Image width + uint16_t height; ///< Image height + uint16_t depth; ///< Image depth + uint8_t cam_no; ///< Camera # (starts with 0) + uint8_t channels; ///< Image channels } mavlink_image_available_t; @@ -199,54 +199,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; - msg.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 0e81a6c6d2..99027fea5b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i * * @param enable 0 to disable, 1 to enable */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg.payload[0]; - - p->enable = enable; // uint8_t:0 to disable, 1 to enable - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 6087d35231..250bee3a1b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -133,43 +133,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; - msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index e5519ef6df..34ebf63517 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -6,13 +6,13 @@ typedef struct __mavlink_marker_t { - uint16_t id; ///< ID float x; ///< x position float y; ///< y position float z; ///< z position float roll; ///< roll orientation float pitch; ///< pitch orientation float yaw; ///< yaw orientation + uint16_t id; ///< ID } mavlink_marker_t; @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_marker_t *p = (mavlink_marker_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_MARKER_LEN; - msg.msgid = MAVLINK_MSG_ID_MARKER; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 311e3be974..b2dd6c3a0c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -6,9 +6,9 @@ typedef struct __mavlink_pattern_detected_t { - uint8_t type; ///< 0: Pattern, 1: Letter float confidence; ///< Confidence of detection - char file[100]; ///< Pattern file name + uint8_t type; ///< 0: Pattern, 1: Letter + uint8_t file[100]; ///< Pattern file name uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; @@ -26,14 +26,14 @@ 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 uint8_t* 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 + memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); @@ -51,14 +51,14 @@ 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 uint8_t* 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 + memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); @@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param file Pattern file name * @param detected Accepted as true detection, 0 no, 1 yes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; - msg.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 uint8_t* file, uint8_t detected) { mavlink_header_t hdr; mavlink_pattern_detected_t payload; @@ -124,7 +98,7 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin 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 + memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes hdr.STX = MAVLINK_STX; @@ -176,7 +150,7 @@ 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, uint8_t* file) { mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; 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 284bd4a21b..0a70ad8e2a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -1,22 +1,22 @@ // MESSAGE POINT_OF_INTEREST PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 42 -#define MAVLINK_MSG_161_LEN 42 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43 +#define MAVLINK_MSG_161_LEN 43 typedef struct __mavlink_point_of_interest_t { - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds float x; ///< X Position float y; ///< Y Position float z; ///< Z Position - char name[25]; ///< POI name + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + uint8_t name[26]; ///< POI name } mavlink_point_of_interest_t; -#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 25 +#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 /** * @brief Pack a point_of_interest message @@ -34,7 +34,7 @@ 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 uint8_t* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; @@ -46,7 +46,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin 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[25]:POI name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -67,7 +67,7 @@ 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 uint8_t* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id 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[25]:POI name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -110,40 +110,10 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param z Z Position * @param name POI name */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg.payload[0]; - - 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[25]:POI name - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; - msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 uint8_t* name) { mavlink_header_t hdr; mavlink_point_of_interest_t payload; @@ -157,7 +127,7 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui 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[25]:POI name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; @@ -263,7 +233,7 @@ 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, uint8_t* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; 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 9724506927..c9fe383afd 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -1,25 +1,25 @@ // MESSAGE POINT_OF_INTEREST_CONNECTION PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 54 -#define MAVLINK_MSG_162_LEN 54 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 +#define MAVLINK_MSG_162_LEN 55 typedef struct __mavlink_point_of_interest_connection_t { - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds float xp1; ///< X1 Position float yp1; ///< Y1 Position float zp1; ///< Z1 Position float xp2; ///< X2 Position float yp2; ///< Y2 Position float zp2; ///< Z2 Position - char name[25]; ///< POI connection name + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + uint8_t name[26]; ///< POI connection name } mavlink_point_of_interest_connection_t; -#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 25 +#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 /** * @brief Pack a point_of_interest_connection message @@ -40,7 +40,7 @@ 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 uint8_t* 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; @@ -55,7 +55,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys 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[25]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI connection name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } @@ -79,7 +79,7 @@ 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 uint8_t* 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; @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ 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[25]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI connection name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } @@ -128,43 +128,10 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param zp2 Z2 Position * @param name POI connection name */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg.payload[0]; - 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[25]:POI connection name - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; - msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 uint8_t* name) { mavlink_header_t hdr; mavlink_point_of_interest_connection_t payload; @@ -181,7 +148,7 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel 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[25]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI connection name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; @@ -320,7 +287,7 @@ 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, uint8_t* name) { mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; 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 2b508a62a5..b5446931f4 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 @@ -6,12 +6,12 @@ typedef struct __mavlink_position_control_offset_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID float x; ///< x position offset float y; ///< y position offset float z; ///< z position offset float yaw; ///< yaw orientation offset in radians, 0 = NORTH + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_offset_set_t; @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 ee33580eb9..dd3b92ca8c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -6,11 +6,11 @@ typedef struct __mavlink_position_control_setpoint_t { - uint16_t id; ///< ID of waypoint, 0 for plain position float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position } mavlink_position_control_setpoint_t; @@ -91,36 +91,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 031a73b01b..f14a31c467 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 @@ -6,13 +6,13 @@ typedef struct __mavlink_position_control_setpoint_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t id; ///< ID of waypoint, 0 for plain position float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_setpoint_set_t; @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 32b82e5686..7bafeb4c99 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -6,13 +6,13 @@ typedef struct __mavlink_raw_aux_t { + int32_t baro; ///< Barometric pressure (hecto Pascal) uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) uint16_t vbat; ///< Battery voltage int16_t temp; ///< Temperature (degrees celcius) - int32_t baro; ///< Barometric pressure (hecto Pascal) } mavlink_raw_aux_t; @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg.payload[0]; - 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) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RAW_AUX_LEN; - msg.msgid = MAVLINK_MSG_ID_RAW_AUX; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 5db1aa478b..5b14662365 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -6,12 +6,12 @@ typedef struct __mavlink_set_cam_shutter_t { + float gain; ///< Camera gain + uint16_t interval; ///< Shutter interval, in microseconds + uint16_t exposure; ///< Exposure time, in microseconds uint8_t cam_no; ///< Camera id uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain } mavlink_set_cam_shutter_t; @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 c27e22bc2b..862ea8cbef 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; - msg.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 dbf29c50ab..cfd0b15f7c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; - msg.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 569f7123b9..e3d0755de3 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; - msg.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index a039a8acaa..dee5bcf0aa 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -6,9 +6,9 @@ typedef struct __mavlink_watchdog_command_t { - uint8_t target_system_id; ///< Target system ID uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID + uint8_t target_system_id; ///< Target system ID uint8_t command_id; ///< Command ID } mavlink_watchdog_command_t; @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index 4d5c2687e4..e31a060ead 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg.payload[0]; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 44a5cf2434..12273bcdd2 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -6,11 +6,11 @@ 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) + uint8_t name[100]; ///< Process name + uint8_t arguments[147]; ///< Process arguments } mavlink_watchdog_process_info_t; #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 @@ -29,15 +29,15 @@ 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 uint8_t* name, const uint8_t* 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 + memcpy(p->name, name, sizeof(p->name)); // uint8_t[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); @@ -56,15 +56,15 @@ 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 uint8_t* name, const uint8_t* 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 + memcpy(p->name, name, sizeof(p->name)); // uint8_t[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); @@ -93,37 +93,10 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param arguments Process arguments * @param timeout Timeout (seconds) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg.payload[0]; - 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) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -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 uint8_t* name, const uint8_t* arguments, int32_t timeout) { mavlink_header_t hdr; mavlink_watchdog_process_info_t payload; @@ -132,8 +105,8 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan 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 + memcpy(p->name, name, sizeof(p->name)); // uint8_t[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) hdr.STX = MAVLINK_STX; @@ -185,7 +158,7 @@ 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, uint8_t* name) { mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; @@ -198,7 +171,7 @@ 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, uint8_t* arguments) { mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; 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 01c80eb317..f6f3dd26d7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -6,12 +6,12 @@ typedef struct __mavlink_watchdog_process_status_t { + int32_t pid; ///< PID uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID + uint16_t crashes; ///< Number of crashes uint8_t state; ///< Is running / finished / suspended / crashed uint8_t muted; ///< Is muted - int32_t pid; ///< PID - uint16_t crashes; ///< Number of crashes } mavlink_watchdog_process_status_t; @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 618dbe87c8..c2d8beffc5 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -66,6 +66,13 @@ 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 { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index 1379932f27..04bffe2e31 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "slugs.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 19503c1df2..b345be106d 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg.payload[0]; - - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_AIR_DATA_LEN; - msg.msgid = MAVLINK_MSG_ID_AIR_DATA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index d3191546a1..f30e1d8b62 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -6,9 +6,9 @@ typedef struct __mavlink_cpu_load_t { + uint16_t batVolt; ///< Battery Voltage in millivolts uint8_t sensLoad; ///< Sensor DSC Load uint8_t ctrlLoad; ///< Control DSC Load - uint16_t batVolt; ///< Battery Voltage in millivolts } mavlink_cpu_load_t; @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; - msg.msgid = MAVLINK_MSG_ID_CPU_LOAD; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 f983a544cf..81b5fa59a2 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -6,8 +6,8 @@ typedef struct __mavlink_ctrl_srfc_pt_t { - uint8_t target; ///< The system setting the commands uint16_t bitfieldPt; ///< Bitfield containing the PT configuration + uint8_t target; ///< The system setting the commands } mavlink_ctrl_srfc_pt_t; @@ -73,33 +73,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; - msg.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index 018de9224d..14da8b9a99 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DATA_LOG_LEN; - msg.msgid = MAVLINK_MSG_ID_DATA_LOG; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index 4bfebe5fb4..e9aa6279b9 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; - msg.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 ca3b1a934e..bab695cfa4 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 c154613328..0436ae9498 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -6,10 +6,10 @@ typedef struct __mavlink_mid_lvl_cmds_t { - uint8_t target; ///< The system setting the commands float hCommand; ///< Commanded Airspeed float uCommand; ///< Log value 2 float rCommand; ///< Log value 3 + uint8_t target; ///< The system setting the commands } mavlink_mid_lvl_cmds_t; @@ -85,35 +85,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg.payload[0]; - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; - msg.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 944ef8d41e..5601a2a344 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -97,37 +97,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg.payload[0]; - 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) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; - msg.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index d860666897..fe3399e2ec 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -6,9 +6,9 @@ typedef struct __mavlink_slugs_action_t { + uint16_t actionVal; ///< Value associated with the action uint8_t target; ///< The system reporting the action uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - uint16_t actionVal; ///< Value associated with the action } mavlink_slugs_action_t; @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg.payload[0]; - - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; - msg.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index 0c8d87f005..dc69df79e8 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -115,40 +115,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg.payload[0]; - 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 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; - msg.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index e742be3584..990857c729 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -43,6 +43,13 @@ extern "C" { #include "./mavlink_msg_mid_lvl_cmds.h" #include "./mavlink_msg_ctrl_srfc_pt.h" #include "./mavlink_msg_slugs_action.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 39a4c44a6b..0bb9f2f906 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:12 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "ualberta.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index d278887fe6..d1b777ee46 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -103,38 +103,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg.payload[0]; - 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] - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; - msg.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 0c5e7972ef..22b139afc6 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -103,37 +103,9 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param pitch Pitch curve setpoints (every 25%) * @param throttle Throttle curve setpoints (every 25%) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg.payload[0]; - - 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%) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; - msg.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; 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 5546cdc5f3..16e2ff6853 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -79,34 +79,9 @@ 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_message_t msg; - uint16_t checksum; - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg.payload[0]; - - 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 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#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; diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index b957f6d73a..c0c03d8279 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:12 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -66,6 +66,13 @@ enum UALBERTA_PILOT_MODE #include "./mavlink_msg_nav_filter_bias.h" #include "./mavlink_msg_radio_calibration.h" #include "./mavlink_msg_ualberta_sys_status.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 73, 239, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/message_definitions/ardupilotmega.xml b/thirdParty/mavlink/message_definitions/ardupilotmega.xml index 39d96eab51..72bf88a7cf 100644 --- a/thirdParty/mavlink/message_definitions/ardupilotmega.xml +++ b/thirdParty/mavlink/message_definitions/ardupilotmega.xml @@ -1,6 +1,6 @@ - common.xml - - + common.xml + + diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index de21876a88..3ca5f52751 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -701,7 +701,7 @@ Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. System ID Component ID - Onboard parameter id + Onboard parameter id Parameter index. Send -1 to use the param ID field as identifier @@ -711,7 +711,7 @@ Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id + Onboard parameter id Onboard parameter value Total number of onboard parameters Index of this onboard parameter @@ -720,12 +720,13 @@ Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. System ID Component ID - Onboard parameter id + Onboard parameter id Onboard parameter value The global position, as returned by the Global Positioning System (GPS). This is -NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. Latitude in 1E7 degrees @@ -818,7 +819,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se The global position, as returned by the Global Positioning System (GPS). This is -NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. Latitude in degrees @@ -876,7 +878,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se 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 + the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed + System ID Component ID Sequence @@ -996,6 +999,22 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se y position 2 / Longitude 2 z position 2 / Altitude 2 + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. 1: enabled, 0: disabled @@ -1014,7 +1033,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Outputs of the APM navigation controller. The primary use of this message is to check the response and signs -of the controller before actual flight and to assist with tuning controller parameters + of the controller before actual flight and to assist with tuning controller parameters + Current desired roll in degrees Current desired pitch in degrees Current desired heading in degrees @@ -1054,6 +1074,27 @@ of the controller before actual flight and to assist with tuning controller para The requested interval between two messages of this type 1 to start sending, 0 to stop sending. + + This packet is useful for high throughput + applications such as hardware in the loop. + + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + The system to be controlled roll @@ -1065,6 +1106,19 @@ of the controller before actual flight and to assist with tuning controller para yaw auto:0, manual:1 thrust auto:0, manual:1 + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + System ID + Component ID + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) Latitude, expressed as * 1E7 @@ -1120,7 +1174,7 @@ of the controller before actual flight and to assist with tuning controller para Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character + Status text message, without null termination character Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. @@ -1128,4 +1182,5 @@ of the controller before actual flight and to assist with tuning controller para DEBUG value +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f diff --git a/thirdParty/mavlink/message_definitions/minimal.xml b/thirdParty/mavlink/message_definitions/minimal.xml index 16d26831e1..5b41a49090 100644 --- a/thirdParty/mavlink/message_definitions/minimal.xml +++ b/thirdParty/mavlink/message_definitions/minimal.xml @@ -1,13 +1,13 @@ - 2 - - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - + 2 + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index 46f18b1544..3878726aa5 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -1,262 +1,235 @@ - + -common.xml - - - - Content Types for data transmission handshake - - - - - - - - - - - - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - - Camera id - Camera mode: 0 = auto, 1 = manual - Trigger pin, 0-3 for PtGrey FireFly - Shutter interval, in microseconds - Exposure time, in microseconds - Camera gain - - - - Timestamp - IMU seq - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - Local frame Z coordinate (height over ground) - GPS X coordinate - GPS Y coordinate - Global frame altitude - Ground truth X - Ground truth Y - Ground truth Z - - - - 0 to disable, 1 to enable - - - - Camera id - Camera # (starts with 0) - Timestamp - Until which timestamp this buffer will stay valid - The image sequence number - Position of the image in the buffer, starts with 0 - Image width - Image height - Image depth - Image channels - Shared memory area key - Exposure time, in microseconds - Camera gain - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - Local frame Z coordinate (height over ground) - GPS X coordinate - GPS Y coordinate - Global frame altitude - Ground truth X - Ground truth Y - Ground truth Z - - - - Timestamp (milliseconds) - Global X position - Global Y position - Global Z position - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - - - - Timestamp (milliseconds) - Global X position - Global Y position - Global Z position - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - - - - Timestamp (milliseconds) - Global X speed - Global Y speed - Global Z speed - - - - Message sent to the MAV to set a new position as reference for the controller - System ID - Component ID - ID of waypoint, 0 for plain position - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - Message sent to the MAV to set a new offset from the currently controlled position - System ID - Component ID - x position offset - y position offset - z position offset - yaw orientation offset in radians, 0 = NORTH - - - - - ID of waypoint, 0 for plain position - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - ID - x position - y position - z position - roll orientation - pitch orientation - yaw orientation - - - - ADC1 (J405 ADC3, LPC2148 AD0.6) - ADC2 (J405 ADC5, LPC2148 AD0.2) - ADC3 (J405 ADC6, LPC2148 AD0.1) - ADC4 (J405 ADC7, LPC2148 AD1.3) - Battery voltage - Temperature (degrees celcius) - Barometric pressure (hecto Pascal) - - - - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Number of I2C errors since startup -Number of I2C errors since startup - Number of I2C errors since startup -Number of I2C errors since startup - Number of I2C errors since startup - - - - Watchdog ID - Number of processes - - - - Watchdog ID - Process ID - Process name - Process arguments - Timeout (seconds) - - - - Watchdog ID - Process ID - Is running / finished / suspended / crashed - Is muted - PID - Number of crashes - - - - Target system ID - Watchdog ID - Process ID - Command ID - - - - 0: Pattern, 1: Letter - Confidence of detection - Pattern file name - Accepted as true detection, 0 no, 1 yes - - - - Notifies the operator about a point of interest (POI). This can be anything detected by the - system. This generic message is intented to help interfacing to generic visualizations and to display - the POI on a map. - 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - 0: global, 1:local - 0: no timeout, >1: timeout in seconds - X Position - Y Position - Z Position - POI name - - - - Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the - system. This generic message is intented to help interfacing to generic visualizations and to display - the POI on a map. - 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - 0: global, 1:local - 0: no timeout, >1: timeout in seconds - X1 Position - Y1 Position - Z1 Position - X2 Position - Y2 Position - Z2 Position - POI connection name - - - - type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - total data size in bytes (set on ACK only) - number of packets beeing sent (set on ACK only) - payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - JPEG quality out of [1,100] - - - - sequence number (starting with 0 on every transmission) - image data bytes - - - - x position in m - y position in m - z position in m - Orientation assignment 0: false, 1:true - Size in pixels - Orientation - Descriptor - Harris operator response at this location - - - + common.xml + + + Content Types for data transmission handshake + + + + + + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + Camera id + Camera mode: 0 = auto, 1 = manual + Trigger pin, 0-3 for PtGrey FireFly + Shutter interval, in microseconds + Exposure time, in microseconds + Camera gain + + + Timestamp + IMU seq + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + Local frame Z coordinate (height over ground) + GPS X coordinate + GPS Y coordinate + Global frame altitude + Ground truth X + Ground truth Y + Ground truth Z + + + 0 to disable, 1 to enable + + + Camera id + Camera # (starts with 0) + Timestamp + Until which timestamp this buffer will stay valid + The image sequence number + Position of the image in the buffer, starts with 0 + Image width + Image height + Image depth + Image channels + Shared memory area key + Exposure time, in microseconds + Camera gain + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + Local frame Z coordinate (height over ground) + GPS X coordinate + GPS Y coordinate + Global frame altitude + Ground truth X + Ground truth Y + Ground truth Z + + + Timestamp (milliseconds) + Global X position + Global Y position + Global Z position + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + + + Timestamp (milliseconds) + Global X position + Global Y position + Global Z position + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + + + Timestamp (milliseconds) + Global X speed + Global Y speed + Global Z speed + + + Message sent to the MAV to set a new position as reference for the controller + System ID + Component ID + ID of waypoint, 0 for plain position + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Message sent to the MAV to set a new offset from the currently controlled position + System ID + Component ID + x position offset + y position offset + z position offset + yaw orientation offset in radians, 0 = NORTH + + + + ID of waypoint, 0 for plain position + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + ID + x position + y position + z position + roll orientation + pitch orientation + yaw orientation + + + ADC1 (J405 ADC3, LPC2148 AD0.6) + ADC2 (J405 ADC5, LPC2148 AD0.2) + ADC3 (J405 ADC6, LPC2148 AD0.1) + ADC4 (J405 ADC7, LPC2148 AD1.3) + Battery voltage + Temperature (degrees celcius) + Barometric pressure (hecto Pascal) + + + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + + + Watchdog ID + Number of processes + + + Watchdog ID + Process ID + Process name + Process arguments + Timeout (seconds) + + + Watchdog ID + Process ID + Is running / finished / suspended / crashed + Is muted + PID + Number of crashes + + + Target system ID + Watchdog ID + Process ID + Command ID + + + 0: Pattern, 1: Letter + Confidence of detection + Pattern file name + Accepted as true detection, 0 no, 1 yes + + + Notifies the operator about a point of interest (POI). This can be anything detected by the + system. This generic message is intented to help interfacing to generic visualizations and to display + the POI on a map. + + 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + 0: global, 1:local + 0: no timeout, >1: timeout in seconds + X Position + Y Position + Z Position + POI name + + + Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the + system. This generic message is intented to help interfacing to generic visualizations and to display + the POI on a map. + + 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + 0: global, 1:local + 0: no timeout, >1: timeout in seconds + X1 Position + Y1 Position + Z1 Position + X2 Position + Y2 Position + Z2 Position + POI connection name + + + type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + total data size in bytes (set on ACK only) + number of packets beeing sent (set on ACK only) + payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + JPEG quality out of [1,100] + + + sequence number (starting with 0 on every transmission) + image data bytes + + + x position in m + y position in m + z position in m + Orientation assignment 0: false, 1:true + Size in pixels + Orientation + Descriptor + Harris operator response at this location + + diff --git a/thirdParty/mavlink/message_definitions/slugs.xml b/thirdParty/mavlink/message_definitions/slugs.xml index 6de0697fef..f8644c5c4b 100644 --- a/thirdParty/mavlink/message_definitions/slugs.xml +++ b/thirdParty/mavlink/message_definitions/slugs.xml @@ -1,8 +1,8 @@ -common.xml - - - - - - - Sensor and DSC control loads. - Sensor DSC Load - Control DSC Load - Battery Voltage in millivolts - - - - Air data for altitude and airspeed computation. - Dynamic pressure (Pa) - Static pressure (Pa) - Board temperature - - - - Accelerometer and gyro biases. - Accelerometer X bias (m/s) - Accelerometer Y bias (m/s) - Accelerometer Z bias (m/s) - Gyro X bias (rad/s) - Gyro Y bias (rad/s) - Gyro Z bias (rad/s) - - - - Configurable diagnostic messages. - Diagnostic float 1 - Diagnostic float 2 - Diagnostic float 3 - Diagnostic short 1 - Diagnostic short 2 - Diagnostic short 3 - - - - Data used in the navigation algorithm. - Measured Airspeed prior to the Nav Filter - Commanded Roll - Commanded Pitch - Commanded Turn rate - Y component of the body acceleration - Total Distance to Run on this leg of Navigation - Remaining distance to Run on this leg of Navigation - Origin WP - Destination WP - - - - Configurable data log probes to be used inside Simulink - Log value 1 - Log value 2 - Log value 3 - Log value 4 - Log value 5 - Log value 6 - - - - Pilot console PWM messges. - Year reported by Gps - Month reported by Gps - Day reported by Gps - Hour reported by Gps - Min reported by Gps - Sec reported by Gps - Visible sattelites reported by Gps - - - Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX. - The system setting the commands - Commanded Airspeed - Log value 2 - Log value 3 - - - -This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: - Position Bit Code - ================================= - 15-8 Reserved - 7 dt_pass 128 - 6 dla_pass 64 - 5 dra_pass 32 - 4 dr_pass 16 - 3 dle_pass 8 - 2 dre_pass 4 - 1 dlf_pass 2 - 0 drf_pass 1 - Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface. - The system setting the commands - Bitfield containing the PT configuration - - - - - Action messages focused on the SLUGS AP. - The system reporting the action - Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - Value associated with the action - - - - \ No newline at end of file + + + + Sensor and DSC control loads. + Sensor DSC Load + Control DSC Load + Battery Voltage in millivolts + + + + Air data for altitude and airspeed computation. + Dynamic pressure (Pa) + Static pressure (Pa) + Board temperature + + + + Accelerometer and gyro biases. + Accelerometer X bias (m/s) + Accelerometer Y bias (m/s) + Accelerometer Z bias (m/s) + Gyro X bias (rad/s) + Gyro Y bias (rad/s) + Gyro Z bias (rad/s) + + + + Configurable diagnostic messages. + Diagnostic float 1 + Diagnostic float 2 + Diagnostic float 3 + Diagnostic short 1 + Diagnostic short 2 + Diagnostic short 3 + + + + Data used in the navigation algorithm. + Measured Airspeed prior to the Nav Filter + Commanded Roll + Commanded Pitch + Commanded Turn rate + Y component of the body acceleration + Total Distance to Run on this leg of Navigation + Remaining distance to Run on this leg of Navigation + Origin WP + Destination WP + + + + Configurable data log probes to be used inside Simulink + Log value 1 + Log value 2 + Log value 3 + Log value 4 + Log value 5 + Log value 6 + + + + Pilot console PWM messges. + Year reported by Gps + Month reported by Gps + Day reported by Gps + Hour reported by Gps + Min reported by Gps + Sec reported by Gps + Visible sattelites reported by Gps + + + + Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX. + The system setting the commands + Commanded Airspeed + Log value 2 + Log value 3 + + + + + This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: + Position Bit Code + ================================= + 15-8 Reserved + 7 dt_pass 128 + 6 dla_pass 64 + 5 dra_pass 32 + 4 dr_pass 16 + 3 dle_pass 8 + 2 dre_pass 4 + 1 dlf_pass 2 + 0 drf_pass 1 + Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface. + The system setting the commands + Bitfield containing the PT configuration + + + + + + Action messages focused on the SLUGS AP. + The system reporting the action + Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + Value associated with the action + + + + diff --git a/thirdParty/mavlink/message_definitions/ualberta.xml b/thirdParty/mavlink/message_definitions/ualberta.xml index eaa9d99844..5e53e141e9 100644 --- a/thirdParty/mavlink/message_definitions/ualberta.xml +++ b/thirdParty/mavlink/message_definitions/ualberta.xml @@ -1,54 +1,54 @@ - common.xml - - - Available autopilot modes for ualberta uav - Raw input pulse widts sent to output - Inputs are normalized using calibration, the converted back to raw pulse widths for output - dfsdfs - dfsfds - dfsdfsdfs - - - Navigation filter mode - - AHRS mode - INS/GPS initialization mode - INS/GPS mode - - - Mode currently commanded by pilot - sdf - dfs - Rotomotion mode - - - - - Accelerometer and Gyro biases from the navigation filter - Timestamp (microseconds) - b_f[0] - b_f[1] - b_f[2] - b_f[0] - b_f[1] - b_f[2] - - - Complete set of calibration parameters for the radio - Aileron setpoints: left, center, right - Elevator setpoints: nose down, center, nose up - Rudder setpoints: nose left, center, nose right - Tail gyro mode/gain setpoints: heading hold, rate mode - Pitch curve setpoints (every 25%) - Throttle curve setpoints (every 25%) - - - System status specific to ualberta uav - System mode, see UALBERTA_AUTOPILOT_MODE ENUM - Navigation mode, see UALBERTA_NAV_MODE ENUM - Pilot mode, see UALBERTA_PILOT_MODE - - + common.xml + + + Available autopilot modes for ualberta uav + Raw input pulse widts sent to output + Inputs are normalized using calibration, the converted back to raw pulse widths for output + dfsdfs + dfsfds + dfsdfsdfs + + + Navigation filter mode + + AHRS mode + INS/GPS initialization mode + INS/GPS mode + + + Mode currently commanded by pilot + sdf + dfs + Rotomotion mode + + + + + Accelerometer and Gyro biases from the navigation filter + Timestamp (microseconds) + b_f[0] + b_f[1] + b_f[2] + b_f[0] + b_f[1] + b_f[2] + + + Complete set of calibration parameters for the radio + Aileron setpoints: left, center, right + Elevator setpoints: nose down, center, nose up + Rudder setpoints: nose left, center, nose right + Tail gyro mode/gain setpoints: heading hold, rate mode + Pitch curve setpoints (every 25%) + Throttle curve setpoints (every 25%) + + + System status specific to ualberta uav + System mode, see UALBERTA_AUTOPILOT_MODE ENUM + Navigation mode, see UALBERTA_NAV_MODE ENUM + Pilot mode, see UALBERTA_PILOT_MODE + + diff --git a/thirdParty/mavlink/missionlib/testing/.gitignore b/thirdParty/mavlink/missionlib/testing/.gitignore index 4b4c3f6c4e..0776965a63 100644 --- a/thirdParty/mavlink/missionlib/testing/.gitignore +++ b/thirdParty/mavlink/missionlib/testing/.gitignore @@ -1,2 +1,3 @@ *.o udptest +build diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c new file mode 100644 index 0000000000..60b653a798 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -0,0 +1,1221 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + and Bryan Godbolt godbolt ( a t ) ualberta.ca + + adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ +/* + This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets + cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from + qgroundcontrol are printed by this program along with the heartbeats. + + + I compiled this program sucessfully on Ubuntu 10.04 with the following command + + gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c + + the rt library is needed for the clock_gettime on linux + */ +/* These headers are for QNX, but should all be standard on unix/linux */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if (defined __QNX__) | (defined __QNXNTO__) +/* QNX specific headers */ +#include +#else +/* Linux / MacOS POSIX timer headers */ +#include +#include +#include +#endif + +#include +<<<<<<< HEAD +#include +======= +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f + +#include <../mavlink_types.h> + +mavlink_system_t mavlink_system; + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +<<<<<<< HEAD +======= +/* Provide the interface functions for the waypoint manager */ + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + printf("SENT %d bytes", bytes_sent); +} + + +#include + + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +char help[] = "--help"; + + +char target_ip[100]; + +float position[6] = {}; +int sock; +struct sockaddr_in gcAddr; +struct sockaddr_in locAddr; +uint8_t buf[BUFFER_LENGTH]; +ssize_t recsize; +socklen_t fromlen; +int bytes_sent; +mavlink_message_t msg; +uint16_t len; +int i = 0; +unsigned int temp = 0; + +uint64_t microsSinceEpoch(); + + +<<<<<<< HEAD +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 + + +struct mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + uint16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +mavlink_wpm_storage wpm; + +bool debug = true; +bool verbose = true; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + printf("SENT %d bytes", bytes_sent); +} + +void mavlink_wpm_send_gcs_string(const char* string) +{ + printf("%s",string); +} + +uint64_t mavlink_wpm_get_system_timestamp() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = wpm.current_partner_sysid; + wpa.target_component = wpm.current_partner_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) + { + //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t position_control_set_point; + + // Send new NED or ENU setpoint to onbaord autopilot + if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) + { + position_control_set_point.target_system = mavlink_system.sysid; + position_control_set_point.target_component = MAV_COMP_ID_IMU; + position_control_set_point.x = cur->x; + position_control_set_point.y = cur->y; + position_control_set_point.z = cur->z; + position_control_set_point.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = wpm.current_partner_sysid; + wpc.target_component = wpm.current_partner_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.size) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); + wp->target_system = wpm.current_partner_sysid; + wp->target_component = wpm.current_partner_compid; + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.max_size) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = wpm.current_partner_sysid; + wpr.target_component = wpm.current_partner_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm.size) +// { +// mavlink_waypoint_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// return (C-A).length(); +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } + return -1.f; +} + + +static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + // Handle param messages + //paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_count = 0; + wpm.current_partner_sysid = 0; + wpm.current_partner_compid = 0; + wpm.current_wp_id = -1; + + if(wpm.size == 0) + { + wpm.current_active_wp_id = -1; + } + } + + if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) + { + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = wpm.accept_range_yaw; + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + wpm.yaw_reached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + wpm.yaw_reached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + wpm.yaw_reached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + wpm.pos_reached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + // FIXME segment distance + //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) + { + wpm.pos_reached = true; + } + } + } + break; + } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm.size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpm.current_wp_id == wpm.size-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_wp_id = 0; + } + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + if (wpc.seq < wpm.size) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + wpm.current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (i == wpm.current_active_wp_id) + { + wpm.waypoints[i].current = true; + } + else + { + wpm.waypoints[i].current = false; + } + } + if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpm.size > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + } + else + { + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + wpm.current_count = wpm.size; + mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm.current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); + else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + wpm.current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); + else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if (verbose) printf("GOT WAYPOINT!"); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + + wpm.current_wp_id = wp.seq + 1; + + if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + + if (wpm.current_active_wp_id > wpm.rcv_size-1) + { + wpm.current_active_wp_id = wpm.rcv_size-1; + } + + // switch the waypoints list + // FIXME CHECK!!! + for (int i = 0; i < wpm.current_count; ++i) + { + wpm.waypoints[i] = wpm.rcv_waypoints[i]; + } + wpm.size = wpm.current_count; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (wpm.waypoints[i].current == 1) + { + wpm.current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm.size) + { + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + wpm.timestamp_firstinside_orbit = 0; + } + + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + } + else + { + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + } + else + { + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { + if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + } + else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + wpm.timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm.size = 0; + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + } + else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + } + break; + } + + default: + { + if (debug) printf("Waypoint: received message of unknown type"); + break; + } + } + + //check if the current waypoint was reached + if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) + { + if (wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if (wpm.timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm.timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + wpm.current_active_wp_id = 1; + } + else + { + if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) + wpm.current_active_wp_id++; + } + + // Fly to next waypoint + wpm.timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.waypoints[wpm.current_active_wp_id].current = true; + wpm.pos_reached = false; + wpm.yaw_reached = false; + if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + } + } + } + } + else + { + wpm.timestamp_lastoutside_orbit = now; + } +} + + + + + + + + +======= +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +int main(int argc, char* argv[]) +{ + // Initialize MAVLink + mavlink_wpm_init(&wpm); + mavlink_system.sysid = 1; + mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; + + + + // Create socket + sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + + // Check if --help flag was used + if ((argc == 2) && (strcmp(argv[1], help) == 0)) + { + printf("\n"); + printf("\tUsage:\n\n"); + printf("\t"); + printf("%s", argv[0]); + printf(" \n"); + printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); + exit(EXIT_FAILURE); + } + + + // Change the target ip if parameter was given + strcpy(target_ip, "127.0.0.1"); + if (argc == 2) + { + strcpy(target_ip, argv[1]); + } + + + memset(&locAddr, 0, sizeof(locAddr)); + locAddr.sin_family = AF_INET; + locAddr.sin_addr.s_addr = INADDR_ANY; + locAddr.sin_port = htons(14551); + + /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ + if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) + { + perror("error bind failed"); + close(sock); + exit(EXIT_FAILURE); + } + + /* Attempt to make it non blocking */ + if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); + close(sock); + exit(EXIT_FAILURE); + } + + + memset(&gcAddr, 0, sizeof(gcAddr)); + gcAddr.sin_family = AF_INET; + gcAddr.sin_addr.s_addr = inet_addr(target_ip); + gcAddr.sin_port = htons(14550); + + + printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); + + + for (;;) + { + + /*Send Heartbeat */ + mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send Status */ + mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + +<<<<<<< HEAD +// /* Send Local Position */ +// mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), +// position[0], position[1], position[2], +// position[3], position[4], position[5]); +// len = mavlink_msg_to_send_buffer(buf, &msg); +// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); +// +// /* Send attitude */ +// mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); +// len = mavlink_msg_to_send_buffer(buf, &msg); +// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); +// +======= + /* Send Local Position */ + mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), + position[0], position[1], position[2], + position[3], position[4], position[5]); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send attitude */ + mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f + + memset(buf, 0, BUFFER_LENGTH); + recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); + if (recsize > 0) + { + // Something received - print out all bytes and parse packet + mavlink_message_t msg; + mavlink_status_t status; + + printf("Bytes Received: %d\nDatagram: ", (int)recsize); + for (i = 0; i < recsize; ++i) + { + temp = buf[i]; + printf("%02x ", (unsigned char)temp); + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // Packet received + printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); + + // Handle packet with waypoint component + mavlink_wpm_message_handler(&msg); + + // Handle packet with parameter component + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + sleep(1); // Sleep one second + } +} + + +/* QNX timer version */ +#if (defined __QNX__) | (defined __QNXNTO__) +uint64_t microsSinceEpoch() +{ + + struct timespec time; + + uint64_t micros = 0; + + clock_gettime(CLOCK_REALTIME, &time); + micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; + + return micros; +} +#else +uint64_t microsSinceEpoch() +{ + + struct timeval tv; + + uint64_t micros = 0; + + gettimeofday(&tv, NULL); + micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + + return micros; +} +#endif \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c index 37a8a7848e..dc096be51b 100644 --- a/thirdParty/mavlink/missionlib/testing/udp.c +++ b/thirdParty/mavlink/missionlib/testing/udp.c @@ -1,5 +1,9 @@ /******************************************************************************* - Copyright (C) 2010 Bryan Godbolt godbolt ( a t ) ualberta.ca + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + and Bryan Godbolt godbolt ( a t ) ualberta.ca + + adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -23,7 +27,7 @@ I compiled this program sucessfully on Ubuntu 10.04 with the following command - gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c + gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c the rt library is needed for the clock_gettime on linux */ @@ -57,6 +61,860 @@ uint64_t microsSinceEpoch(); + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 + +struct _mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t count; + MAVLINK_WPM_STATES current_state; +} mavlink_wpm_storage; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->count = 0; + state->current_state = MAVLINK_WPM_STATE_IDLE; +} + + +PX_WAYPOINTPLANNER_STATES current_state = PX_WPP_IDLE; +uint16_t protocol_current_wp_id = 0; +uint16_t protocol_current_count = 0; +uint8_t protocol_current_partner_systemid = 0; +uint8_t protocol_current_partner_compid = 0; +uint64_t protocol_timestamp_lastaction = 0; + +uint64_t timestamp_last_send_setpoint = 0; + + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = target_systemid; + wpa.target_component = target_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t PControlSetPoint; + + // send new set point to local IMU + if (cur->frame == 1) + { + PControlSetPoint.target_system = systemid; + PControlSetPoint.target_component = MAV_COMP_ID_IMU; + PControlSetPoint.x = cur->x; + PControlSetPoint.y = cur->y; + PControlSetPoint.z = cur->z; + PControlSetPoint.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + timestamp_last_send_setpoint = now; + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = target_systemid; + wpc.target_component = target_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) +{ + if (seq < waypoints->size()) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = waypoints->at(seq); + wp->target_system = target_systemid; + wp->target_component = target_compid; + mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) +{ + if (seq < waypoints->size()) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = target_systemid; + wpr.target_component = target_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +{ + if (seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + const PxVector3 A(cur->x, cur->y, cur->z); + const PxVector3 C(x, y, z); + + // seq not the second last waypoint + if ((uint16_t)(seq+1) < waypoints->size()) + { + mavlink_waypoint_t *next = waypoints->at(seq+1); + const PxVector3 B(next->x, next->y, next->z); + const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); + if (r >= 0 && r <= 1) + { + const PxVector3 P(A + r*(B-A)); + return (P-C).length(); + } + else if (r < 0.f) + { + return (C-A).length(); + } + else + { + return (C-B).length(); + } + } + else + { + return (C-A).length(); + } + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } + return -1.f; +} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ + if (seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + const PxVector3 A(cur->x, cur->y, cur->z); + const PxVector3 C(x, y, z); + + return (C-A).length(); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } + return -1.f; +} + + +static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user) +{ + // Handle param messages + paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); + current_state = PX_WPP_IDLE; + protocol_current_count = 0; + protocol_current_partner_systemid = 0; + protocol_current_partner_compid = 0; + protocol_current_wp_id = -1; + + if(waypoints->size() == 0) + { + current_active_wp_id = -1; + } + } + + if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size()) + { + send_setpoint(current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); + if(wp->frame == 1) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE"); + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + yawReached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + yawReached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + yawReached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); + + if(wp->frame == 1) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + posReached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && yawReached) + { + posReached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_CMD: // special action from ground station + { + mavlink_cmd_t action; + mavlink_msg_cmd_decode(msg, &action); + if(action.target == systemid) + { + if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; + switch (action.action) + { + // case MAV_ACTION_LAUNCH: + // if (verbose) std::cerr << "Launch received" << std::endl; + // current_active_wp_id = 0; + // if (waypoints->size()>0) + // { + // setActive(waypoints[current_active_wp_id]); + // } + // else + // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; + // break; + + // case MAV_ACTION_CONTINUE: + // if (verbose) std::c + // err << "Continue received" << std::endl; + // idle = false; + // setActive(waypoints[current_active_wp_id]); + // break; + + // case MAV_ACTION_HALT: + // if (verbose) std::cerr << "Halt received" << std::endl; + // idle = true; + // break; + + // default: + // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; + // break; + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) + { + if (protocol_current_wp_id == waypoints->size()-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); + current_state = PX_WPP_IDLE; + protocol_current_wp_id = 0; + } + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == systemid && wpc.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE) + { + if (wpc.seq < waypoints->size()) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < waypoints->size(); i++) + { + if (i == current_active_wp_id) + { + waypoints->at(i)->current = true; + } + else + { + waypoints->at(i)->current = false; + } + } + if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + yawReached = false; + posReached = false; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == systemid && wprl.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) + { + if (waypoints->size() > 0) + { + if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); + current_state = PX_WPP_SENDLIST; + protocol_current_wp_id = 0; + protocol_current_partner_systemid = msg->sysid; + protocol_current_partner_compid = msg->compid; + } + else + { + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + protocol_current_count = waypoints->size(); + send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) + { + protocol_timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) + { + if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + current_state = PX_WPP_SENDLIST_SENDWPS; + protocol_current_wp_id = wpr.seq; + send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } + else if (current_state == PX_WPP_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (current_state == PX_WPP_SENDLIST_SENDWPS) + { + if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); + else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == systemid && wpc.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + current_state = PX_WPP_GETLIST; + protocol_current_wp_id = 0; + protocol_current_partner_systemid = msg->sysid; + protocol_current_partner_compid = msg->compid; + protocol_current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + while(waypoints_receive_buffer->size() > 0) + { + delete waypoints_receive_buffer->back(); + waypoints_receive_buffer->pop_back(); + } + + send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n"); + while(waypoints_receive_buffer->size() > 0) + { + delete waypoints->back(); + waypoints->pop_back(); + } + current_active_wp_id = -1; + yawReached = false; + posReached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); + else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) + { + protocol_timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) + { + if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + current_state = PX_WPP_GETLIST_GETWPS; + protocol_current_wp_id = wp.seq + 1; + mavlink_waypoint_t* newwp = new mavlink_waypoint_t; + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + waypoints_receive_buffer->push_back(newwp); + + if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); + + send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); + + if (current_active_wp_id > waypoints_receive_buffer->size()-1) + { + current_active_wp_id = waypoints_receive_buffer->size() - 1; + } + + // switch the waypoints list + std::vector* waypoints_temp = waypoints; + waypoints = waypoints_receive_buffer; + waypoints_receive_buffer = waypoints_temp; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < waypoints->size(); i++) + { + if (waypoints->at(i)->current == 1) + { + current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + yawReached = false; + posReached = false; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == waypoints->size()) + { + current_active_wp_id = -1; + yawReached = false; + posReached = false; + timestamp_firstinside_orbit = 0; + } + + current_state = PX_WPP_IDLE; + } + else + { + send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); + } + } + else + { + if (current_state == PX_WPP_IDLE) + { + //we're done receiving waypoints, answer with ack. + send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } + else if (current_state == PX_WPP_GETLIST) + { + if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else if (current_state == PX_WPP_GETLIST_GETWPS) + { + if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); + else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); + } + else if(wp.target_system == systemid && wp.target_component == compid) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) + { + protocol_timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + while(waypoints->size() > 0) + { + delete waypoints->back(); + waypoints->pop_back(); + } + current_active_wp_id = -1; + yawReached = false; + posReached = false; + } + else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); + } + break; + } + + default: + { + if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl; + break; + } + } + + //check if the current waypoint was reached + if ((posReached && /*yawReached &&*/ !idle)) + { + if (current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); + + if (timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + send_waypoint_reached(cur_wp->seq); + timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + current_active_wp_id = 1; + } + else + { + if ((uint16_t)(current_active_wp_id + 1) < waypoints->size()) + current_active_wp_id++; + } + + // Fly to next waypoint + timestamp_firstinside_orbit = 0; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + waypoints->at(current_active_wp_id)->current = true; + posReached = false; + yawReached = false; + if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); + } + } + } + } + else + { + timestamp_lastoutside_orbit = now; + } +} + + + + + + + + int main(int argc, char* argv[]) { @@ -129,6 +987,8 @@ int main(int argc, char* argv[]) gcAddr.sin_port = htons(14550); + printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); + for (;;) { diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c new file mode 100644 index 0000000000..335f593a9e --- /dev/null +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -0,0 +1,880 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +#include + +static mavlink_wpm_storage wpm; + +bool debug = true; +bool verbose = true; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +void mavlink_wpm_send_gcs_string(const char* string) +{ + printf("%s",string); +} + +uint64_t mavlink_wpm_get_system_timestamp() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = wpm.current_partner_sysid; + wpa.target_component = wpm.current_partner_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) + { + //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t position_control_set_point; + + // Send new NED or ENU setpoint to onbaord autopilot + if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) + { + position_control_set_point.target_system = mavlink_system.sysid; + position_control_set_point.target_component = MAV_COMP_ID_IMU; + position_control_set_point.x = cur->x; + position_control_set_point.y = cur->y; + position_control_set_point.z = cur->z; + position_control_set_point.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = wpm.current_partner_sysid; + wpc.target_component = wpm.current_partner_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.size) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); + wp->target_system = wpm.current_partner_sysid; + wp->target_component = wpm.current_partner_compid; + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.max_size) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = wpm.current_partner_sysid; + wpr.target_component = wpm.current_partner_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm.size) +// { +// mavlink_waypoint_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// return (C-A).length(); +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } + return -1.f; +} + + +static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + // Handle param messages + //paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_count = 0; + wpm.current_partner_sysid = 0; + wpm.current_partner_compid = 0; + wpm.current_wp_id = -1; + + if(wpm.size == 0) + { + wpm.current_active_wp_id = -1; + } + } + + if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) + { + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = wpm.accept_range_yaw; + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + wpm.yaw_reached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + wpm.yaw_reached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + wpm.yaw_reached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + wpm.pos_reached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + // FIXME segment distance + //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) + { + wpm.pos_reached = true; + } + } + } + break; + } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm.size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpm.current_wp_id == wpm.size-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_wp_id = 0; + } + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + if (wpc.seq < wpm.size) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + wpm.current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (i == wpm.current_active_wp_id) + { + wpm.waypoints[i].current = true; + } + else + { + wpm.waypoints[i].current = false; + } + } + if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpm.size > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + } + else + { + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + wpm.current_count = wpm.size; + mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm.current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); + else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + wpm.current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); + else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if (verbose) printf("GOT WAYPOINT!"); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + + wpm.current_wp_id = wp.seq + 1; + + if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + + if (wpm.current_active_wp_id > wpm.rcv_size-1) + { + wpm.current_active_wp_id = wpm.rcv_size-1; + } + + // switch the waypoints list + // FIXME CHECK!!! + for (int i = 0; i < wpm.current_count; ++i) + { + wpm.waypoints[i] = wpm.rcv_waypoints[i]; + } + wpm.size = wpm.current_count; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (wpm.waypoints[i].current == 1) + { + wpm.current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm.size) + { + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + wpm.timestamp_firstinside_orbit = 0; + } + + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + } + else + { + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + } + else + { + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { + if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + } + else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + wpm.timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm.size = 0; + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + } + else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + } + break; + } + + default: + { + if (debug) printf("Waypoint: received message of unknown type"); + break; + } + } + + //check if the current waypoint was reached + if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) + { + if (wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if (wpm.timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm.timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + wpm.current_active_wp_id = 1; + } + else + { + if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) + wpm.current_active_wp_id++; + } + + // Fly to next waypoint + wpm.timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.waypoints[wpm.current_active_wp_id].current = true; + wpm.pos_reached = false; + wpm.yaw_reached = false; + if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + } + } + } + } + else + { + wpm.timestamp_lastoutside_orbit = now; + } +} + diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h new file mode 100644 index 0000000000..4bc32a14a4 --- /dev/null +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -0,0 +1,91 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 + + +struct mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + uint16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +void mavlink_wpm_init(mavlink_wpm_storage* state); + +void mavlink_wpm_message_handler(const mavlink_message_t* msg); \ No newline at end of file -- GitLab From d67528e06a8eac957bf74cbefb17f659e12c7fa1 Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 9 Aug 2011 19:02:45 +0200 Subject: [PATCH 013/131] Updated to latest MAVLink --- src/QGCCore.cc | 1 - src/comm/MAVLinkProtocol.cc | 2 - src/comm/MAVLinkSimulationLink.cc | 8 ++-- src/comm/MAVLinkSimulationWaypointPlanner.cc | 10 ++--- src/comm/QGCMAVLink.h | 1 + src/main.cc | 1 + src/uas/PxQuadMAV.cc | 2 +- src/uas/UAS.cc | 10 ++--- thirdParty/mavlink/include/bittest.c | 39 -------------------- 9 files changed, 17 insertions(+), 57 deletions(-) delete mode 100644 thirdParty/mavlink/include/bittest.c diff --git a/src/QGCCore.cc b/src/QGCCore.cc index ca6bf0c9af..c1f460a0c7 100644 --- a/src/QGCCore.cc +++ b/src/QGCCore.cc @@ -44,7 +44,6 @@ This file is part of the QGROUNDCONTROL project #include "configuration.h" #include "QGC.h" #include "QGCCore.h" -#include "MG.h" #include "MainWindow.h" #include "GAudioOutput.h" diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 098fe65ffa..7a2bd1efda 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -17,7 +17,6 @@ #include #include -//#include "MG.h" #include "MAVLinkProtocol.h" #include "UASInterface.h" #include "UASManager.h" @@ -28,7 +27,6 @@ #include "ArduPilotMegaMAV.h" #include "configuration.h" #include "LinkManager.h" -//#include "MainWindow.h" #include "QGCMAVLink.h" #include "QGCMAVLinkUASFactory.h" #include "QGC.h" diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 4ffd5ec4b1..8fb07e8934 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -716,11 +716,11 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } break; // EXECUTE OPERATOR ACTIONS - case MAVLINK_MSG_ID_ACTION: { - mavlink_action_t action; - mavlink_msg_action_decode(&msg, &action); + case MAVLINK_MSG_ID_COMMAND: { + mavlink_command_t action; + mavlink_msg_command_decode(&msg, &action); - qDebug() << "SIM" << "received action" << action.action << "for system" << action.target; + qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system; // FIXME MAVLINKV10PORTINGNEEDED // switch (action.action) { diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index d78b4d5bfe..0f229d2f57 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -815,11 +815,11 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* break; } - case MAVLINK_MSG_ID_ACTION: { // special action from ground station - mavlink_action_t action; - mavlink_msg_action_decode(msg, &action); - if(action.target == systemid) { - if (verbose) qDebug("Waypoint: received message with action %d\n", action.action); + case MAVLINK_MSG_ID_COMMAND: { // special action from ground station + mavlink_command_t action; + mavlink_msg_command_decode(msg, &action); + if(action.target_system == systemid) { + if (verbose) qDebug("Waypoint: received message with action %d\n", action.command); // switch (action.action) { // case MAV_ACTION_LAUNCH: // if (verbose) std::cerr << "Launch received" << std::endl; diff --git a/src/comm/QGCMAVLink.h b/src/comm/QGCMAVLink.h index b03e72e271..3debe72f25 100644 --- a/src/comm/QGCMAVLink.h +++ b/src/comm/QGCMAVLink.h @@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project #ifndef QGCMAVLINK_H #define QGCMAVLINK_H +#define MAVLINK_NO_DATA #include #include diff --git a/src/main.cc b/src/main.cc index 910c57e6bc..b4ff08262f 100644 --- a/src/main.cc +++ b/src/main.cc @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCCore.h" #include "MainWindow.h" #include "configuration.h" +#include "mavlink_data.h" /* SDL does ugly things to main() */ diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 1ad0fbfe80..478d5d87d8 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -65,7 +65,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_pattern_detected_decode(&message, &detected); QByteArray b; b.resize(256); - mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data()); + mavlink_msg_pattern_detected_get_file(&message, b.data()); b.append('\0'); QString name = QString(b); if (detected.type == 0) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index ff463e28ae..3efaebda2d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -809,16 +809,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val); } break; - case MAVLINK_MSG_ID_ACTION_ACK: - mavlink_action_ack_t ack; - mavlink_msg_action_ack_decode(&message, &ack); + case MAVLINK_MSG_ID_CMD_ACK: + mavlink_cmd_ack_t ack; + mavlink_msg_cmd_ack_decode(&message, &ack); if (ack.result == 1) { - emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action)); + emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed CMD: %1").arg(ack.cmd)); } else { - emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action)); + emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected CMD: %1").arg(ack.cmd)); } break; case MAVLINK_MSG_ID_DEBUG: diff --git a/thirdParty/mavlink/include/bittest.c b/thirdParty/mavlink/include/bittest.c deleted file mode 100644 index c966b72afb..0000000000 --- a/thirdParty/mavlink/include/bittest.c +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include - -int main(int argc, char* argv[]) -{ - - uint8_t bitfield = 254; // 11111110 - - uint8_t mask = 128; // 10000000 - - uint8_t result = (bitfield & mask); - - printf("0x%02x\n", bitfield); - - // Transform into network order - - generic_32bit bin; - bin.i = 1; - printf("First byte in (little endian) 0x%02x\n", bin.b[0]); - generic_32bit bout; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - printf("Last byte out (big endian) 0x%02x\n", bout.b[3]); - - uint8_t n = 5; - printf("Mask is 0x%02x\n", ((uint32_t)(1 << n)) - 1); // = 2^n - 1 - - int32_t encoded = 2; - uint8_t bits = 2; - - uint8_t packet[MAVLINK_MAX_PACKET_LEN]; - uint8_t packet_index = 0; - uint8_t bit_index = 0; - - put_bitfield_n_by_index(encoded, bits, packet_index, bit_index, &bit_index, packet); - -} -- GitLab From 959501133c1968fcad28ffeb3dfd7f9375cc96be Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 9 Aug 2011 19:04:42 +0200 Subject: [PATCH 014/131] Updated packed MAVLink sources --- thirdParty/mavlink/include/common/common.h | 7 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../include/common/mavlink_msg_cmd_ack.h | 142 ++++++++++++++++++ .../common/mavlink_msg_param_request_read.h | 16 +- .../include/common/mavlink_msg_param_set.h | 16 +- .../include/common/mavlink_msg_param_value.h | 16 +- .../include/common/mavlink_msg_statustext.h | 16 +- thirdParty/mavlink/include/mavlink_options.h | 4 +- thirdParty/mavlink/include/mavlink_types.h | 142 ------------------ thirdParty/mavlink/include/pixhawk/mavlink.h | 2 +- .../pixhawk/mavlink_msg_pattern_detected.h | 16 +- .../pixhawk/mavlink_msg_point_of_interest.h | 16 +- ...mavlink_msg_point_of_interest_connection.h | 16 +- .../mavlink_msg_watchdog_process_info.h | 26 ++-- thirdParty/mavlink/include/pixhawk/pixhawk.h | 4 +- thirdParty/mavlink/include/protocol.h | 19 +-- .../mavlink/message_definitions/common.xml | 18 +-- .../mavlink/message_definitions/pixhawk.xml | 10 +- 18 files changed, 241 insertions(+), 247 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index a366e7b2df..e01dff73f4 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Tuesday, August 9 2011, 16:49 UTC */ #ifndef COMMON_H #define COMMON_H @@ -236,8 +236,7 @@ enum MAV_CMD #include "./mavlink_msg_change_operator_control.h" #include "./mavlink_msg_change_operator_control_ack.h" #include "./mavlink_msg_auth_key.h" -#include "./mavlink_msg_action_ack.h" -#include "./mavlink_msg_action.h" +#include "./mavlink_msg_cmd_ack.h" #include "./mavlink_msg_set_mode.h" #include "./mavlink_msg_set_nav_mode.h" #include "./mavlink_msg_param_request_read.h" @@ -300,7 +299,7 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 144 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 29d8958ff1..d4aa65e5ff 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Tuesday, August 9 2011, 16:49 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h new file mode 100644 index 0000000000..b2e7609656 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h @@ -0,0 +1,142 @@ +// MESSAGE CMD_ACK PACKING + +#define MAVLINK_MSG_ID_CMD_ACK 9 +#define MAVLINK_MSG_ID_CMD_ACK_LEN 2 +#define MAVLINK_MSG_9_LEN 2 + +typedef struct __mavlink_cmd_ack_t +{ + uint8_t cmd; ///< The MAV_CMD ID + uint8_t result; ///< 0: Action DENIED, 1: Action executed + +} mavlink_cmd_ack_t; + +/** + * @brief Pack a cmd_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param cmd The MAV_CMD ID + * @param result 0: Action DENIED, 1: Action executed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cmd_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t cmd, uint8_t result) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_CMD_ACK; + + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_ACK_LEN); +} + +/** + * @brief Pack a cmd_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param cmd The MAV_CMD ID + * @param result 0: Action DENIED, 1: Action executed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cmd_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t cmd, uint8_t result) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_CMD_ACK; + + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_ACK_LEN); +} + +/** + * @brief Encode a cmd_ack struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param cmd_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_ack_t* cmd_ack) +{ + return mavlink_msg_cmd_ack_pack(system_id, component_id, msg, cmd_ack->cmd, cmd_ack->result); +} + +/** + * @brief Send a cmd_ack message + * @param chan MAVLink channel to send the message + * + * @param cmd The MAV_CMD ID + * @param result 0: Action DENIED, 1: Action executed + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_cmd_ack_send(mavlink_channel_t chan, uint8_t cmd, uint8_t result) +{ + mavlink_header_t hdr; + mavlink_cmd_ack_t payload; + uint16_t checksum; + mavlink_cmd_ack_t *p = &payload; + + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CMD_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_CMD_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE CMD_ACK UNPACKING + +/** + * @brief Get field cmd from cmd_ack message + * + * @return The MAV_CMD ID + */ +static inline uint8_t mavlink_msg_cmd_ack_get_cmd(const mavlink_message_t* msg) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + return (uint8_t)(p->cmd); +} + +/** + * @brief Get field result from cmd_ack message + * + * @return 0: Action DENIED, 1: Action executed + */ +static inline uint8_t mavlink_msg_cmd_ack_get_result(const mavlink_message_t* msg) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + return (uint8_t)(p->result); +} + +/** + * @brief Decode a cmd_ack message into a struct + * + * @param msg The message to decode + * @param cmd_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_cmd_ack_decode(const mavlink_message_t* msg, mavlink_cmd_ack_t* cmd_ack) +{ + memcpy( cmd_ack, msg->payload, sizeof(mavlink_cmd_ack_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h index 07e3ae044e..86c0a4b1d3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -9,7 +9,7 @@ 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 - uint8_t param_id[16]; ///< Onboard parameter id + char param_id[16]; ///< Onboard parameter id } mavlink_param_request_read_t; #define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 @@ -26,14 +26,14 @@ 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 uint8_t* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { 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)); // uint8_t[16]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); @@ -51,14 +51,14 @@ 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 uint8_t* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { 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)); // uint8_t[16]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, #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 uint8_t* param_id, int16_t param_index) +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; @@ -98,7 +98,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u 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)); // uint8_t[16]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier hdr.STX = MAVLINK_STX; @@ -150,7 +150,7 @@ 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, uint8_t* 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]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index 396a5307e6..7dfb5b6dcc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -9,7 +9,7 @@ typedef struct __mavlink_param_set_t float param_value; ///< Onboard parameter value uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint8_t param_id[16]; ///< Onboard parameter id + char param_id[16]; ///< Onboard parameter id } mavlink_param_set_t; #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 @@ -26,14 +26,14 @@ typedef struct __mavlink_param_set_t * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { 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)); // uint8_t[16]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); @@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { 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)); // uint8_t[16]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c #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 uint8_t* param_id, float param_value) +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { mavlink_header_t hdr; mavlink_param_set_t payload; @@ -98,7 +98,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta 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)); // uint8_t[16]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value hdr.STX = MAVLINK_STX; @@ -150,7 +150,7 @@ 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, uint8_t* 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]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 636589cd07..19f618ab44 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -9,7 +9,7 @@ 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 - uint8_t param_id[16]; ///< Onboard parameter id + char param_id[16]; ///< Onboard parameter id } mavlink_param_value_t; #define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 @@ -26,12 +26,12 @@ 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 uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { 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)); // uint8_t[16]:Onboard parameter 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_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -51,12 +51,12 @@ 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 uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { 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)); // uint8_t[16]:Onboard parameter 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_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -89,14 +89,14 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_header_t hdr; mavlink_param_value_t payload; uint16_t checksum; mavlink_param_value_t *p = &payload; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter 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_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -128,7 +128,7 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ui * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, uint8_t* 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]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index bc9c77b8fa..e53bc5390e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -7,7 +7,7 @@ typedef struct __mavlink_statustext_t { uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault - int8_t text[50]; ///< Status text message, without null termination character + char text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 @@ -22,13 +22,13 @@ typedef struct __mavlink_statustext_t * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const int8_t* text) +static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const char* text) { 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)); // int8_t[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -43,13 +43,13 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const int8_t* text) +static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const char* text) { 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)); // int8_t[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text) +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; @@ -85,7 +85,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s mavlink_statustext_t *p = &payload; p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; @@ -125,7 +125,7 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_ * * @return Status text message, without null termination character */ -static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t* 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]; diff --git a/thirdParty/mavlink/include/mavlink_options.h b/thirdParty/mavlink/include/mavlink_options.h index 646ebed02a..bc337f6176 100755 --- a/thirdParty/mavlink/include/mavlink_options.h +++ b/thirdParty/mavlink/include/mavlink_options.h @@ -19,7 +19,7 @@ extern "C" { * table. Comment out the define to leave out the table and code to check it. * */ -#define MAVLINK_CHECK_LENGTH +//#define MAVLINK_CHECK_LENGTH /** * @@ -30,7 +30,7 @@ extern "C" { * enable if you make the changes required. Default DISABLED. * */ -//#define MAVLINK_STAIC_BUFFER +//#define MAVLINK_STATIC_BUFFER /** * diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index 3124ba621d..8232c104d7 100755 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -9,148 +9,6 @@ #include "inttypes.h" -enum MAV_CLASS -{ - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch - MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu - MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NB ///< Number of autopilot classes -}; - -enum MAV_ACTION -{ - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions -}; - -enum MAV_MODE -{ - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back -}; - -enum MAV_STATE -{ - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_POWEROFF -}; - -enum MAV_NAV -{ - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER -}; - -enum MAV_TYPE -{ - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6 -}; - -enum MAV_AUTOPILOT_TYPE -{ - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3 -}; - -enum MAV_COMPONENT -{ - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 -}; - -enum MAV_FRAME -{ - MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 -}; - #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 diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index c59c58b83e..b14edfc4d0 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Tuesday, August 9 2011, 16:49 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index b2dd6c3a0c..d5d803bd0c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -8,7 +8,7 @@ typedef struct __mavlink_pattern_detected_t { float confidence; ///< Confidence of detection uint8_t type; ///< 0: Pattern, 1: Letter - uint8_t file[100]; ///< Pattern file name + char file[100]; ///< Pattern file name uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; @@ -26,14 +26,14 @@ 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 uint8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { 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)); // uint8_t[100]:Pattern file name + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); @@ -51,14 +51,14 @@ 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 uint8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { 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)); // uint8_t[100]:Pattern file name + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const uint8_t* file, uint8_t detected) +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; @@ -98,7 +98,7 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin p->type = type; // uint8_t:0: Pattern, 1: Letter p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes hdr.STX = MAVLINK_STX; @@ -150,7 +150,7 @@ 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, uint8_t* 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]; 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 0a70ad8e2a..148d8f784e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -13,7 +13,7 @@ typedef struct __mavlink_point_of_interest_t uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta uint8_t coordinate_system; ///< 0: global, 1:local - uint8_t name[26]; ///< POI name + char name[26]; ///< POI name } mavlink_point_of_interest_t; #define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 @@ -34,7 +34,7 @@ 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 uint8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; @@ -46,7 +46,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin 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)); // uint8_t[26]:POI name + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -67,7 +67,7 @@ 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 uint8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id 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)); // uint8_t[26]:POI name + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u #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 uint8_t* 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) { mavlink_header_t hdr; mavlink_point_of_interest_t payload; @@ -127,7 +127,7 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui 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)); // uint8_t[26]:POI name + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; @@ -233,7 +233,7 @@ 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, uint8_t* 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]; 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 c9fe383afd..7accdbc9c3 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 @@ -16,7 +16,7 @@ typedef struct __mavlink_point_of_interest_connection_t uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta uint8_t coordinate_system; ///< 0: global, 1:local - uint8_t name[26]; ///< POI connection name + char name[26]; ///< POI connection name } mavlink_point_of_interest_connection_t; #define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 @@ -40,7 +40,7 @@ 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 uint8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { 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; @@ -55,7 +55,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys 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)); // uint8_t[26]:POI connection name + 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); } @@ -79,7 +79,7 @@ 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 uint8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { 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; @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ 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)); // uint8_t[26]:POI connection name + 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); } @@ -131,7 +131,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s #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 uint8_t* 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) { mavlink_header_t hdr; mavlink_point_of_interest_connection_t payload; @@ -148,7 +148,7 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel 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)); // uint8_t[26]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; @@ -287,7 +287,7 @@ 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, uint8_t* 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]; 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 12273bcdd2..e9d7703f9e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -9,8 +9,8 @@ typedef struct __mavlink_watchdog_process_info_t int32_t timeout; ///< Timeout (seconds) uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID - uint8_t name[100]; ///< Process name - uint8_t arguments[147]; ///< Process arguments + char name[100]; ///< Process name + char arguments[147]; ///< Process arguments } mavlink_watchdog_process_info_t; #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 @@ -29,15 +29,15 @@ 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 uint8_t* name, const uint8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { 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)); // uint8_t[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); @@ -56,15 +56,15 @@ 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 uint8_t* name, const uint8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { 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)); // uint8_t[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); @@ -96,7 +96,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i #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 uint8_t* name, const uint8_t* arguments, int32_t timeout) +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; @@ -105,8 +105,8 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan 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)); // uint8_t[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) hdr.STX = MAVLINK_STX; @@ -158,7 +158,7 @@ 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, uint8_t* 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]; @@ -171,7 +171,7 @@ 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, uint8_t* 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]; diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index c2d8beffc5..a0bd474b19 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Tuesday, August 9 2011, 16:49 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -71,7 +71,7 @@ enum DATA_TYPES // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 144 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/protocol.h b/thirdParty/mavlink/include/protocol.h index c69a872cc3..a45993e692 100644 --- a/thirdParty/mavlink/include/protocol.h +++ b/thirdParty/mavlink/include/protocol.h @@ -322,18 +322,19 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag status->parse_state = MAVLINK_PARSE_STATE_IDLE; if (c == MAVLINK_STX) { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); } - } + } else { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if ( r_message != NULL ) - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - 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; } diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 3ca5f52751..4846bbaf29 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -676,17 +676,11 @@ Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. key - + This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The action id + The MAV_CMD ID 0: Action DENIED, 1: Action executed - - An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The system executing the action - The component executing the action - The action id - Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. The system setting the mode @@ -701,7 +695,7 @@ Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. System ID Component ID - Onboard parameter id + Onboard parameter id Parameter index. Send -1 to use the param ID field as identifier @@ -711,7 +705,7 @@ Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id + Onboard parameter id Onboard parameter value Total number of onboard parameters Index of this onboard parameter @@ -720,7 +714,7 @@ Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. System ID Component ID - Onboard parameter id + Onboard parameter id Onboard parameter value @@ -1174,7 +1168,7 @@ Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character + Status text message, without null termination character Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index 3878726aa5..0eb3bb638e 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -155,8 +155,8 @@ Watchdog ID Process ID - Process name - Process arguments + Process name + Process arguments Timeout (seconds) @@ -176,7 +176,7 @@ 0: Pattern, 1: Letter Confidence of detection - Pattern file name + Pattern file name Accepted as true detection, 0 no, 1 yes @@ -191,7 +191,7 @@ X Position Y Position Z Position - POI name + POI name Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the @@ -208,7 +208,7 @@ X2 Position Y2 Position Z2 Position - POI connection name + POI connection name type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) -- GitLab From a9d925f871825085289b3f887a537b17f34e8662 Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 9 Aug 2011 19:10:10 +0200 Subject: [PATCH 015/131] Minor MAVLink cleanups, might compile now - but not guaranteed --- qgroundcontrol.pro | 1 + src/comm/MAVLinkProtocol.cc | 3 + src/comm/QGCMAVLink.h | 1 - src/main.cc | 1 - .../include/common/mavlink_msg_action.h | 160 ------------------ .../include/common/mavlink_msg_action_ack.h | 142 ---------------- 6 files changed, 4 insertions(+), 304 deletions(-) delete mode 100644 thirdParty/mavlink/include/common/mavlink_msg_action.h delete mode 100644 thirdParty/mavlink/include/common/mavlink_msg_action_ack.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b85e7a7bbd..3d3097797b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -38,6 +38,7 @@ MOC_DIR = $$BUILDDIR/moc UI_HEADERS_DIR = $$BUILDDIR/ui RCC_DIR = $$BUILDDIR/rcc MAVLINK_CONF = "" +DEFINES += MAVLINK_NO_DATA ################################################################# diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 7a2bd1efda..ebb6d2eaa0 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -31,6 +31,9 @@ #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. diff --git a/src/comm/QGCMAVLink.h b/src/comm/QGCMAVLink.h index 3debe72f25..b03e72e271 100644 --- a/src/comm/QGCMAVLink.h +++ b/src/comm/QGCMAVLink.h @@ -30,7 +30,6 @@ This file is part of the QGROUNDCONTROL project #ifndef QGCMAVLINK_H #define QGCMAVLINK_H -#define MAVLINK_NO_DATA #include #include diff --git a/src/main.cc b/src/main.cc index b4ff08262f..910c57e6bc 100644 --- a/src/main.cc +++ b/src/main.cc @@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project #include "QGCCore.h" #include "MainWindow.h" #include "configuration.h" -#include "mavlink_data.h" /* SDL does ugly things to main() */ diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action.h b/thirdParty/mavlink/include/common/mavlink_msg_action.h deleted file mode 100644 index 636689544f..0000000000 --- a/thirdParty/mavlink/include/common/mavlink_msg_action.h +++ /dev/null @@ -1,160 +0,0 @@ -// MESSAGE ACTION PACKING - -#define MAVLINK_MSG_ID_ACTION 10 -#define MAVLINK_MSG_ID_ACTION_LEN 3 -#define MAVLINK_MSG_10_LEN 3 - -typedef struct __mavlink_action_t -{ - uint8_t target; ///< The system executing the action - uint8_t target_component; ///< The component executing the action - uint8_t action; ///< The action id - -} mavlink_action_t; - -/** - * @brief Pack a action message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target The system executing the action - * @param target_component The component executing the action - * @param action The action id - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action) -{ - mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; - msg->msgid = MAVLINK_MSG_ID_ACTION; - - p->target = target; // uint8_t:The system executing the action - p->target_component = target_component; // uint8_t:The component executing the action - p->action = action; // uint8_t:The action id - - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTION_LEN); -} - -/** - * @brief Pack a action message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system executing the action - * @param target_component The component executing the action - * @param action The action id - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action) -{ - mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; - msg->msgid = MAVLINK_MSG_ID_ACTION; - - p->target = target; // uint8_t:The system executing the action - p->target_component = target_component; // uint8_t:The component executing the action - p->action = action; // uint8_t:The action id - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTION_LEN); -} - -/** - * @brief Encode a action struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param action C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_t* action) -{ - return mavlink_msg_action_pack(system_id, component_id, msg, action->target, action->target_component, action->action); -} - -/** - * @brief Send a action message - * @param chan MAVLink channel to send the message - * - * @param target The system executing the action - * @param target_component The component executing the action - * @param action The action id - */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) -{ - mavlink_header_t hdr; - mavlink_action_t payload; - uint16_t checksum; - mavlink_action_t *p = &payload; - - p->target = target; // uint8_t:The system executing the action - p->target_component = target_component; // uint8_t:The component executing the action - p->action = action; // uint8_t:The action id - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_ACTION_LEN; - hdr.msgid = MAVLINK_MSG_ID_ACTION; - hdr.sysid = mavlink_system.sysid; - hdr.compid = mavlink_system.compid; - hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; - mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); -} - -#endif -// MESSAGE ACTION UNPACKING - -/** - * @brief Get field target from action message - * - * @return The system executing the action - */ -static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg) -{ - mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; - return (uint8_t)(p->target); -} - -/** - * @brief Get field target_component from action message - * - * @return The component executing the action - */ -static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_message_t* msg) -{ - mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; - return (uint8_t)(p->target_component); -} - -/** - * @brief Get field action from action message - * - * @return The action id - */ -static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg) -{ - mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; - return (uint8_t)(p->action); -} - -/** - * @brief Decode a action message into a struct - * - * @param msg The message to decode - * @param action C-struct to decode the message contents into - */ -static inline void mavlink_msg_action_decode(const mavlink_message_t* msg, mavlink_action_t* action) -{ - memcpy( action, msg->payload, sizeof(mavlink_action_t)); -} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h deleted file mode 100644 index b407815e7e..0000000000 --- a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h +++ /dev/null @@ -1,142 +0,0 @@ -// MESSAGE ACTION_ACK PACKING - -#define MAVLINK_MSG_ID_ACTION_ACK 9 -#define MAVLINK_MSG_ID_ACTION_ACK_LEN 2 -#define MAVLINK_MSG_9_LEN 2 - -typedef struct __mavlink_action_ack_t -{ - uint8_t action; ///< The action id - uint8_t result; ///< 0: Action DENIED, 1: Action executed - -} mavlink_action_ack_t; - -/** - * @brief Pack a action_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param action The action id - * @param result 0: Action DENIED, 1: Action executed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t action, uint8_t result) -{ - mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; - msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - - p->action = action; // uint8_t:The action id - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTION_ACK_LEN); -} - -/** - * @brief Pack a action_ack message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param action The action id - * @param result 0: Action DENIED, 1: Action executed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t action, uint8_t result) -{ - mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; - msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - - p->action = action; // uint8_t:The action id - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTION_ACK_LEN); -} - -/** - * @brief Encode a action_ack struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param action_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_ack_t* action_ack) -{ - return mavlink_msg_action_ack_pack(system_id, component_id, msg, action_ack->action, action_ack->result); -} - -/** - * @brief Send a action_ack message - * @param chan MAVLink channel to send the message - * - * @param action The action id - * @param result 0: Action DENIED, 1: Action executed - */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) -{ - mavlink_header_t hdr; - mavlink_action_ack_t payload; - uint16_t checksum; - mavlink_action_ack_t *p = &payload; - - p->action = action; // uint8_t:The action id - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - - hdr.STX = MAVLINK_STX; - hdr.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; - hdr.msgid = MAVLINK_MSG_ID_ACTION_ACK; - hdr.sysid = mavlink_system.sysid; - hdr.compid = mavlink_system.compid; - hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; - mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); -} - -#endif -// MESSAGE ACTION_ACK UNPACKING - -/** - * @brief Get field action from action_ack message - * - * @return The action id - */ -static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* msg) -{ - mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; - return (uint8_t)(p->action); -} - -/** - * @brief Get field result from action_ack message - * - * @return 0: Action DENIED, 1: Action executed - */ -static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* msg) -{ - mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; - return (uint8_t)(p->result); -} - -/** - * @brief Decode a action_ack message into a struct - * - * @param msg The message to decode - * @param action_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_action_ack_decode(const mavlink_message_t* msg, mavlink_action_ack_t* action_ack) -{ - memcpy( action_ack, msg->payload, sizeof(mavlink_action_ack_t)); -} -- GitLab From 11b3e471ae0be0514999afef570519eae8700f02 Mon Sep 17 00:00:00 2001 From: LM Date: Wed, 10 Aug 2011 09:58:06 +0200 Subject: [PATCH 016/131] Last changes --- src/comm/MAVLinkSimulationLink.cc | 1 + thirdParty/mavlink/include/mavlink_protocol.h | 537 +----------------- thirdParty/mavlink/include/mavlink_types.h | 35 +- 3 files changed, 29 insertions(+), 544 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 8fb07e8934..77cc0d1210 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -556,6 +556,7 @@ void MAVLinkSimulationLink::mainloop() messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_CLASS_PIXHAWK); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + qDebug() << "CRC:" << msg.ck_a << msg.ck_b; //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h index 62163d5983..fe63af913e 100755 --- a/thirdParty/mavlink/include/mavlink_protocol.h +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -116,7 +116,7 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin // 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 + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; } @@ -128,9 +128,9 @@ static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink { *(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; + 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; @@ -149,16 +149,6 @@ union checksum_ { uint8_t c[2]; }; -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - static inline void mavlink_start_checksum(mavlink_message_t* msg) { union checksum_ ck; @@ -213,7 +203,7 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) * * @endcode */ -#ifdef MAVLINK_STAIC_BUFFER +#ifdef MAVLINK_STATIC_BUFFER static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) #else static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) @@ -431,521 +421,4 @@ static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint #define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) #endif -#define FILE_FINISHED - -#ifndef FILE_FINISHED -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ - -#define MAVLINK_PACKET_START_CANDIDATES 50 -/* -static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; - #else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; - static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; - #endif - - // Set a packet start candidate index if sign is start sign - if (c == MAVLINK_STX) - { - m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; - } - - // Parse normally, if a CRC mismatch occurs retry with the next packet index -} -//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; -//#else -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; -//#endif -//// Initializes only once, values keep unchanged after first initialization -// mavlink_parse_state_initialize(&m_mavlink_status[chan]); -// -//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message -//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status -//int bufferIndex = 0; -// -//status->msg_received = 0; -// -//switch (status->parse_state) -//{ -//case MAVLINK_PARSE_STATE_UNINIT: -//case MAVLINK_PARSE_STATE_IDLE: -// if (c == MAVLINK_STX) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; -// mavlink_start_checksum(rxmsg); -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_STX: -// if (status->msg_received) -// { -// status->buffer_overrun++; -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 -// rxmsg->len = c; -// status->packet_idx = 0; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_LENGTH: -// rxmsg->seq = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SEQ: -// rxmsg->sysid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SYSID: -// rxmsg->compid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_COMPID: -// rxmsg->msgid = c; -// mavlink_update_checksum(rxmsg, c); -// if (rxmsg->len == 0) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// else -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_MSGID: -// rxmsg->payload[status->packet_idx++] = c; -// mavlink_update_checksum(rxmsg, c); -// if (status->packet_idx == rxmsg->len) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -// if (c != rxmsg->ck_a) -// { -// // Check first checksum byte -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// 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; -// } -// 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_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; -} - -r_mavlink_status->current_seq = status->current_seq+1; -r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; -r_mavlink_status->packet_rx_drop_count = status->parse_error; -return status->msg_received; -} - */ - - -typedef union __generic_16bit -{ - uint8_t b[2]; - int16_t s; -} generic_16bit; - -typedef union __generic_32bit -{ - uint8_t b[4]; - float f; - int32_t i; - int16_t s; -} generic_32bit; - -typedef union __generic_64bit -{ - uint8_t b[8]; - int64_t ll; ///< Long long (64 bit) -} generic_64bit; - -/** - * @brief Place an unsigned byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = b; - return sizeof(b); -} - -/** - * @brief Place a signed byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = (uint8_t)b; - return sizeof(b); -} - -/** - * @brief Place two unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>8)&0xff; - buffer[bindex+1] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place two signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint16_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>56)&0xff; - buffer[bindex+1] = (b>>48)&0xff; - buffer[bindex+2] = (b>>40)&0xff; - buffer[bindex+3] = (b>>32)&0xff; - buffer[bindex+4] = (b>>24)&0xff; - buffer[bindex+5] = (b>>16)&0xff; - buffer[bindex+6] = (b>>8)&0xff; - buffer[bindex+7] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint64_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place a float into the buffer - * - * @param b the float to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) -{ - generic_32bit g; - g.f = b; - return put_int32_t_by_index(g.i, bindex, buffer); -} - -/** - * @brief Place an array into the buffer - * - * @param b the array to add - * @param length size of the array (for strings: length WITH '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) -{ - memcpy(buffer+bindex, b, length); - return length; -} - -/** - * @brief Place a string into the buffer - * - * @param b the string to add - * @param maxlength size of the array (for strings: length WITHOUT '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) -{ - uint16_t length = 0; - // Copy string into buffer, ensuring not to exceed the buffer size - int i; - for (i = 1; i < maxlength; i++) - { - length++; - // String characters - if (i < (maxlength - 1)) - { - buffer[bindex+i] = b[i]; - // Stop at null character - if (b[i] == '\0') - { - break; - } - } - // Enforce null termination at end of buffer - else if (i == (maxlength - 1)) - { - buffer[i] = '\0'; - } - } - // Write length into first field - put_uint8_t_by_index(length, bindex, buffer); - return length; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - generic_32bit bin; - generic_32bit bout; - uint8_t i_bit_index, i_byte_index, curr_bits_n; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (8 - i_bit_index)) - { - // Enough space - curr_bits_n = bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} -*/ -#endif /* FILE_FINISHED */ - #endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index 8232c104d7..36d8d57c5d 100755 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -22,20 +22,27 @@ #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 __mavlink_system +{ uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function uint8_t type; ///< Unused, can be used by user to store the system's type uint8_t state; ///< Unused, can be used by user to store the system's state uint8_t mode; ///< Unused, can be used by user to store the system's mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode } mavlink_system_t; -typedef struct __mavlink_message { - union { - uint16_t ck; ///< Checksum high byte - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; +typedef struct __mavlink_message +{ + union + { + uint16_t ck; ///< Checksum high byte + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; uint8_t STX; ///< start of packet marker uint8_t len; ///< Length of payload uint8_t seq; ///< Sequence of packet @@ -45,7 +52,8 @@ typedef struct __mavlink_message { uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU } mavlink_message_t; -typedef struct __mavlink_header { +typedef struct __mavlink_header +{ union { uint16_t ck; ///< Checksum high byte uint8_t ck_a; ///< Checksum low byte @@ -59,16 +67,18 @@ typedef struct __mavlink_header { uint8_t msgid; ///< ID of message in payload } mavlink_header_t; -typedef enum { +typedef enum +{ MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, MAVLINK_COMM_3, MAVLINK_COMM_NB, MAVLINK_COMM_NB_HIGH = 16 - } mavlink_channel_t; +} mavlink_channel_t; -typedef enum { +typedef enum +{ MAVLINK_PARSE_STATE_UNINIT=0, MAVLINK_PARSE_STATE_IDLE, MAVLINK_PARSE_STATE_GOT_STX, @@ -81,7 +91,8 @@ typedef enum { MAVLINK_PARSE_STATE_GOT_CRC1 } mavlink_parse_state_t; ///< The state machine for the comm parser -typedef struct __mavlink_status { +typedef struct __mavlink_status +{ uint8_t ck_a; ///< Checksum byte 1 uint8_t ck_b; ///< Checksum byte 2 uint8_t msg_received; ///< Number of received messages -- GitLab From 738a310eff7385742f9fe0768b020f6ce98caaea Mon Sep 17 00:00:00 2001 From: LM Date: Wed, 10 Aug 2011 11:13:18 +0200 Subject: [PATCH 017/131] Updated a few messages --- thirdParty/mavlink/include/common/common.h | 5 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../common/mavlink_msg_global_position_int.h | 46 +++-- .../include/common/mavlink_msg_gps_raw.h | 30 ++- .../include/common/mavlink_msg_gps_raw_int.h | 128 +++++++------ .../include/common/mavlink_msg_memory_vect.h | 181 ++++++++++++++++++ .../mavlink/message_definitions/common.xml | 26 ++- 7 files changed, 332 insertions(+), 86 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index e01dff73f4..32dca2b897 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:49 UTC + * Generated on Wednesday, August 10 2011, 09:07 UTC */ #ifndef COMMON_H #define COMMON_H @@ -289,6 +289,7 @@ enum MAV_CMD #include "./mavlink_msg_vfr_hud.h" #include "./mavlink_msg_command.h" #include "./mavlink_msg_command_ack.h" +#include "./mavlink_msg_memory_vect.h" #include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_named_value_float.h" #include "./mavlink_msg_named_value_int.h" @@ -299,7 +300,7 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 127, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index d4aa65e5ff..76c0dbcf9c 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:49 UTC + * Generated on Wednesday, August 10 2011, 09:07 UTC */ #ifndef MAVLINK_H #define MAVLINK_H 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 cc0f6e4428..9dbc636e20 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -1,17 +1,18 @@ // MESSAGE GLOBAL_POSITION_INT PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 18 -#define MAVLINK_MSG_73_LEN 18 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 20 +#define MAVLINK_MSG_73_LEN 20 typedef struct __mavlink_global_position_int_t { int32_t lat; ///< Latitude, expressed as * 1E7 int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 } mavlink_global_position_int_t; @@ -23,23 +24,25 @@ typedef struct __mavlink_global_position_int_t * * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { 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) + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } @@ -52,23 +55,25 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param msg The MAVLink message to compress the data into * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { 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) + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } @@ -83,7 +88,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) { - return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->vx, global_position_int->vy, global_position_int->vz); + return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); } /** @@ -92,15 +97,16 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +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; @@ -109,10 +115,11 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, 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->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; @@ -161,7 +168,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_mess /** * @brief Get field alt from global_position_int message * - * @return Altitude in meters, expressed as * 1000 (millimeters) + * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL */ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) { @@ -202,6 +209,17 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa return (int16_t)(p->vz); } +/** + * @brief Get field hdg from global_position_int message + * + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) +{ + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (uint16_t)(p->hdg); +} + /** * @brief Decode a global_position_int message into a struct * diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 6435fa3994..0ee5e0f3f7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -1,8 +1,8 @@ // MESSAGE GPS_RAW PACKING #define MAVLINK_MSG_ID_GPS_RAW 32 -#define MAVLINK_MSG_ID_GPS_RAW_LEN 37 -#define MAVLINK_MSG_32_LEN 37 +#define MAVLINK_MSG_ID_GPS_RAW_LEN 38 +#define MAVLINK_MSG_32_LEN 38 typedef struct __mavlink_gps_raw_t { @@ -15,6 +15,7 @@ typedef struct __mavlink_gps_raw_t 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; @@ -33,9 +34,10 @@ typedef struct __mavlink_gps_raw_t * @param epv GPS VDOP * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; @@ -49,6 +51,7 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo 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); } @@ -68,9 +71,10 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo * @param epv GPS VDOP * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; @@ -84,6 +88,7 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t 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); } @@ -98,7 +103,7 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_t* gps_raw) { - return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg); + return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg, gps_raw->satellites_visible); } /** @@ -114,11 +119,12 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @param epv GPS VDOP * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) +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; @@ -134,6 +140,7 @@ static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t use p->epv = epv; // float:GPS VDOP p->v = v; // float:GPS ground speed p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_RAW_LEN; @@ -256,6 +263,17 @@ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) return (float)(p->hdg); } +/** + * @brief Get field satellites_visible from gps_raw message + * + * @return Number of satellites visible + */ +static inline uint8_t mavlink_msg_gps_raw_get_satellites_visible(const mavlink_message_t* msg) +{ + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint8_t)(p->satellites_visible); +} + /** * @brief Decode a gps_raw message into a struct * 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 5ec33bd643..c0cf3bba93 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -1,20 +1,21 @@ // MESSAGE GPS_RAW_INT PACKING #define MAVLINK_MSG_ID_GPS_RAW_INT 25 -#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 37 -#define MAVLINK_MSG_25_LEN 37 +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 +#define MAVLINK_MSG_25_LEN 30 typedef struct __mavlink_gps_raw_int_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) int32_t lat; ///< Latitude in 1E7 degrees int32_t lon; ///< Longitude in 1E7 degrees - int32_t alt; ///< Altitude in 1E3 meters (millimeters) - float eph; ///< GPS HDOP - float epv; ///< GPS VDOP - float v; ///< GPS ground speed (m/s) - float hdg; ///< Compass heading in degrees, 0..360 degrees + int32_t alt; ///< Altitude in 1E3 meters (millimeters) above MSL + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -28,14 +29,15 @@ typedef struct __mavlink_gps_raw_int_t * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude in 1E7 degrees * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed (m/s) - * @param hdg Compass heading in degrees, 0..360 degrees + * @param alt Altitude in 1E3 meters (millimeters) above MSL + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; @@ -44,11 +46,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c 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) - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed (m/s) - p->hdg = hdg; // float:Compass heading in degrees, 0..360 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); } @@ -63,14 +66,15 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude in 1E7 degrees * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed (m/s) - * @param hdg Compass heading in degrees, 0..360 degrees + * @param alt Altitude in 1E3 meters (millimeters) above MSL + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; @@ -79,11 +83,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint 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) - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed (m/s) - p->hdg = hdg; // float:Compass heading in degrees, 0..360 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); } @@ -98,7 +103,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) { - return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->v, gps_raw_int->hdg); + return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->hdg, gps_raw_int->satellites_visible); } /** @@ -109,16 +114,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude in 1E7 degrees * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed (m/s) - * @param hdg Compass heading in degrees, 0..360 degrees + * @param alt Altitude in 1E3 meters (millimeters) above MSL + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) +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; @@ -129,11 +135,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t 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) - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed (m/s) - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; @@ -204,7 +211,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* m /** * @brief Get field alt from gps_raw_int message * - * @return Altitude in 1E3 meters (millimeters) + * @return Altitude in 1E3 meters (millimeters) above MSL */ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) { @@ -215,45 +222,56 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m /** * @brief Get field eph from gps_raw_int message * - * @return GPS HDOP + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (float)(p->eph); + return (uint16_t)(p->eph); } /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP + * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (float)(p->epv); + return (uint16_t)(p->epv); } /** - * @brief Get field v from gps_raw_int message + * @brief Get field vel from gps_raw_int message * - * @return GPS ground speed (m/s) + * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; - return (float)(p->v); + return (uint16_t)(p->vel); } /** * @brief Get field hdg from gps_raw_int message * - * @return Compass heading in degrees, 0..360 degrees + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg) +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 (float)(p->hdg); + return (uint16_t)(p->hdg); +} + +/** + * @brief Get field satellites_visible from gps_raw_int message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline 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); } /** diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h new file mode 100644 index 0000000000..ce90911d16 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -0,0 +1,181 @@ +// MESSAGE MEMORY_VECT PACKING + +#define MAVLINK_MSG_ID_MEMORY_VECT 250 +#define MAVLINK_MSG_ID_MEMORY_VECT_LEN 36 +#define MAVLINK_MSG_250_LEN 36 + +typedef struct __mavlink_memory_vect_t +{ + uint16_t address; ///< Starting address of the debug variables + uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + uint8_t type; ///< Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + int8_t value[32]; ///< Memory contents at specified address + +} mavlink_memory_vect_t; +#define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32 + +/** + * @brief Pack a memory_vect message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; + + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +} + +/** + * @brief Pack a memory_vect message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; + + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +} + +/** + * @brief Encode a memory_vect struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + +/** + * @brief Send a memory_vect message + * @param chan MAVLink channel to send the message + * + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) +{ + mavlink_header_t hdr; + mavlink_memory_vect_t payload; + uint16_t checksum; + mavlink_memory_vect_t *p = &payload; + + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MEMORY_VECT_LEN; + hdr.msgid = MAVLINK_MSG_ID_MEMORY_VECT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE MEMORY_VECT UNPACKING + +/** + * @brief Get field address from memory_vect message + * + * @return Starting address of the debug variables + */ +static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message_t* msg) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + return (uint16_t)(p->address); +} + +/** + * @brief Get field ver from memory_vect message + * + * @return Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + */ +static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* msg) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + return (uint8_t)(p->ver); +} + +/** + * @brief Get field type from memory_vect message + * + * @return Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + */ +static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* msg) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + return (uint8_t)(p->type); +} + +/** + * @brief Get field value from memory_vect message + * + * @return Memory contents at specified address + */ +static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t* value) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + + memcpy(value, p->value, sizeof(p->value)); + return sizeof(p->value); +} + +/** + * @brief Decode a memory_vect message into a struct + * + * @param msg The message to decode + * @param memory_vect C-struct to decode the message contents into + */ +static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* msg, mavlink_memory_vect_t* memory_vect) +{ + memcpy( memory_vect, msg->payload, sizeof(mavlink_memory_vect_t)); +} diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 4846bbaf29..95e92da0e6 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -725,11 +725,12 @@ 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. Latitude in 1E7 degrees Longitude in 1E7 degrees - Altitude in 1E3 meters (millimeters) - GPS HDOP - GPS VDOP - GPS ground speed (m/s) - Compass heading in degrees, 0..360 degrees + Altitude in 1E3 meters (millimeters) above MSL + GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + GPS ground speed (m/s * 100). If unknown, set to: 65535 + Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + Number of satellites visible. If unknown, set to 255 The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units @@ -812,7 +813,7 @@ Z Speed (in Altitude direction, positive: going up) - The global position, as returned by the Global Positioning System (GPS). This is + IMPORTANT: Please use GPS_RAW_INT for maximum precision. The max FLOAT resolution limits the resolution to about 1.8 m on some places on the earth. The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -824,6 +825,7 @@ GPS VDOP GPS ground speed Compass heading in degrees, 0..360 degrees + Number of satellites visible The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. @@ -1117,10 +1119,11 @@ The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) Latitude, expressed as * 1E7 Longitude, expressed as * 1E7 - Altitude in meters, expressed as * 1000 (millimeters) + Altitude in meters, expressed as * 1000 (millimeters), above MSL Ground X Speed (Latitude), expressed as m/s * 100 Ground Y Speed (Longitude), expressed as m/s * 100 Ground Z Speed (Altitude), expressed as m/s * 100 + Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 Metrics typically displayed on a HUD for fixed wing aircraft @@ -1147,7 +1150,14 @@ Current airspeed in m/s 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - + + + Send raw controller memory. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Starting address of the debug variables + Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + Memory contents at specified address + Name Timestamp -- GitLab From 9a705c22cb07241fc042d4fb2bbb8ce98858b057 Mon Sep 17 00:00:00 2001 From: LM Date: Thu, 11 Aug 2011 11:19:13 +0200 Subject: [PATCH 018/131] Updated MAVLink --- thirdParty/mavlink/include/common/common.h | 6 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../mavlink_msg_roll_pitch_yaw_setpoint.h | 178 +++++++++ ...avlink_msg_roll_pitch_yaw_speed_setpoint.h | 178 +++++++++ .../mavlink/message_definitions/common.xml | 14 + thirdParty/mavlink/missionlib/waypoints.c | 372 +++++++++++++----- thirdParty/mavlink/missionlib/waypoints.h | 4 +- 7 files changed, 645 insertions(+), 109 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 32dca2b897..8ce7b21f82 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, August 10 2011, 09:07 UTC + * Generated on Thursday, August 11 2011, 07:25 UTC */ #ifndef COMMON_H #define COMMON_H @@ -275,6 +275,8 @@ enum MAV_CMD #include "./mavlink_msg_safety_allowed_area.h" #include "./mavlink_msg_set_roll_pitch_yaw.h" #include "./mavlink_msg_set_roll_pitch_yaw_speed.h" +#include "./mavlink_msg_roll_pitch_yaw_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_setpoint.h" #include "./mavlink_msg_attitude_controller_output.h" #include "./mavlink_msg_position_controller_output.h" #include "./mavlink_msg_nav_controller_output.h" @@ -300,7 +302,7 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 127, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 79, 252, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 76c0dbcf9c..7de59d0c68 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, August 10 2011, 09:07 UTC + * Generated on Thursday, August 11 2011, 07:25 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h new file mode 100644 index 0000000000..41befa9202 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h @@ -0,0 +1,178 @@ +// MESSAGE ROLL_PITCH_YAW_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT 57 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN 16 +#define MAVLINK_MSG_57_LEN 16 + +typedef struct __mavlink_roll_pitch_yaw_setpoint_t +{ + uint32_t time_ms; ///< Timestamp in milliseconds since system boot + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + +} mavlink_roll_pitch_yaw_setpoint_t; + +/** + * @brief Pack a roll_pitch_yaw_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_ms Timestamp in milliseconds since system boot + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_ms Timestamp in milliseconds since system boot + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_setpoint_t* roll_pitch_yaw_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_setpoint->time_ms, roll_pitch_yaw_setpoint->roll, roll_pitch_yaw_setpoint->pitch, roll_pitch_yaw_setpoint->yaw); +} + +/** + * @brief Send a roll_pitch_yaw_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_ms Timestamp in milliseconds since system boot + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_roll_pitch_yaw_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_roll_pitch_yaw_setpoint_t payload; + uint16_t checksum; + mavlink_roll_pitch_yaw_setpoint_t *p = &payload; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_roll_pitch_yaw_setpoint_get_time_ms(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (uint32_t)(p->time_ms); +} + +/** + * @brief Get field roll from roll_pitch_yaw_setpoint message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_roll(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from roll_pitch_yaw_setpoint message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_pitch(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from roll_pitch_yaw_setpoint message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_yaw(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Decode a roll_pitch_yaw_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_setpoint_t* roll_pitch_yaw_setpoint) +{ + memcpy( roll_pitch_yaw_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_setpoint_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h new file mode 100644 index 0000000000..c920eaf346 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h @@ -0,0 +1,178 @@ +// MESSAGE ROLL_PITCH_YAW_SPEED_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT 58 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN 16 +#define MAVLINK_MSG_58_LEN 16 + +typedef struct __mavlink_roll_pitch_yaw_speed_setpoint_t +{ + uint32_t time_ms; ///< Timestamp in milliseconds since system boot + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + +} mavlink_roll_pitch_yaw_speed_setpoint_t; + +/** + * @brief Pack a roll_pitch_yaw_speed_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_ms Timestamp in milliseconds since system boot + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_speed_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_ms Timestamp in milliseconds since system boot + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_speed_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_setpoint_t* roll_pitch_yaw_speed_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_setpoint->time_ms, roll_pitch_yaw_speed_setpoint->roll_speed, roll_pitch_yaw_speed_setpoint->pitch_speed, roll_pitch_yaw_speed_setpoint->yaw_speed); +} + +/** + * @brief Send a roll_pitch_yaw_speed_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_ms Timestamp in milliseconds since system boot + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_roll_pitch_yaw_speed_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_header_t hdr; + mavlink_roll_pitch_yaw_speed_setpoint_t payload; + uint16_t checksum; + mavlink_roll_pitch_yaw_speed_setpoint_t *p = &payload; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SPEED_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_speed_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_setpoint_get_time_ms(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (uint32_t)(p->time_ms); +} + +/** + * @brief Get field roll_speed from roll_pitch_yaw_speed_setpoint message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_roll_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @brief Get field pitch_speed from roll_pitch_yaw_speed_setpoint message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_pitch_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @brief Get field yaw_speed from roll_pitch_yaw_speed_setpoint message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_yaw_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @brief Decode a roll_pitch_yaw_speed_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_speed_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_setpoint_t* roll_pitch_yaw_speed_setpoint) +{ + memcpy( roll_pitch_yaw_speed_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_speed_setpoint_t)); +} diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 95e92da0e6..4d4c1e52a5 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -1011,6 +1011,20 @@ Desired pitch angular speed in rad/s Desired yaw angular speed in rad/s + + Setpoint in roll, pitch, yaw currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + + + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. 1: enabled, 0: disabled diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c index 335f593a9e..d0e7621f75 100644 --- a/thirdParty/mavlink/missionlib/waypoints.c +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -18,12 +18,20 @@ ****************************************************************************/ #include - -static mavlink_wpm_storage wpm; +#include bool debug = true; bool verbose = true; +extern mavlink_system_t mavlink_system; +extern mavlink_wpm_storage wpm; + +extern void mavlink_wpm_send_message(mavlink_message_t* msg); +extern void mavlink_wpm_send_gcs_string(const char* string); +extern uint64_t mavlink_wpm_get_system_timestamp(); + + +#define MAVLINK_WPM_NO_PRINTF void mavlink_wpm_init(mavlink_wpm_storage* state) { @@ -48,18 +56,6 @@ void mavlink_wpm_init(mavlink_wpm_storage* state) } -void mavlink_wpm_send_gcs_string(const char* string) -{ - printf("%s",string); -} - -uint64_t mavlink_wpm_get_system_timestamp() -{ - struct timeval tv; - gettimeofday(&tv, NULL); - return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; -} - /* * @brief Sends an waypoint ack message */ @@ -79,7 +75,11 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) if (MAVLINK_WPM_TEXT_FEEDBACK) { - //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +#endif mavlink_wpm_send_gcs_string("Sent waypoint ACK"); } } @@ -109,7 +109,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); } else { @@ -151,14 +151,14 @@ void mavlink_wpm_send_setpoint(uint16_t seq) } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//// if (verbose) // printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); } wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); } } @@ -174,7 +174,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -189,7 +189,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) wp->target_component = wpm.current_partner_compid; mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -210,7 +210,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s wpr.seq = seq; mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -237,7 +237,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -278,7 +278,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // } // else // { -// if (verbose) printf("ERROR: index out of bounds\n"); +// // if (verbose) // printf("ERROR: index out of bounds\n"); // } // return -1.f; //} @@ -296,25 +296,27 @@ float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) // } // else // { -// if (verbose) printf("ERROR: index out of bounds\n"); +// // if (verbose) // printf("ERROR: index out of bounds\n"); // } return -1.f; } -static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +void mavlink_wpm_message_handler(const mavlink_message_t* msg) { // Handle param messages //paramClient->handleMAVLinkPacket(msg); //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + uint64_t now = mavlink_wpm_get_system_timestamp(); if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { - if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Operation timeout switching -> IDLE"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); +#endif + wpm.current_state = MAVLINK_WPM_STATE_IDLE; wpm.current_count = 0; wpm.current_partner_sysid = 0; wpm.current_partner_compid = 0; @@ -376,7 +378,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_local_position_t pos; mavlink_msg_local_position_decode(msg, &pos); - //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); wpm.pos_reached = false; @@ -409,34 +411,34 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) // mavlink_msg_cmd_decode(msg, &action); // if(action.target == mavlink_system.sysid) // { -// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; // switch (action.action) // { // // case MAV_ACTION_LAUNCH: -// // if (verbose) std::cerr << "Launch received" << std::endl; +// // // if (verbose) std::cerr << "Launch received" << std::endl; // // current_active_wp_id = 0; // // if (wpm.size>0) // // { // // setActive(waypoints[current_active_wp_id]); // // } // // else -// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; // // break; // // // case MAV_ACTION_CONTINUE: -// // if (verbose) std::c +// // // if (verbose) std::c // // err << "Continue received" << std::endl; // // idle = false; // // setActive(waypoints[current_active_wp_id]); // // break; // // // case MAV_ACTION_HALT: -// // if (verbose) std::cerr << "Halt received" << std::endl; +// // // if (verbose) std::cerr << "Halt received" << std::endl; // // idle = true; // // break; // // // default: -// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; // // break; // } // } @@ -448,7 +450,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_ack_t wpa; mavlink_msg_waypoint_ack_decode(msg, &wpa); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/)) { wpm.timestamp_lastaction = now; @@ -456,15 +458,23 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpm.current_wp_id == wpm.size-1) { - if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Got last WP ACK state -> IDLE"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); +#endif + wpm.current_state = MAVLINK_WPM_STATE_IDLE; wpm.current_wp_id = 0; } } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } break; } @@ -474,7 +484,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_set_current_t wpc; mavlink_msg_waypoint_set_current_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; @@ -482,7 +492,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpc.seq < wpm.size) { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + // if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); wpm.current_active_wp_id = wpc.seq; uint32_t i; for(i = 0; i < wpm.size; i++) @@ -496,7 +506,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.waypoints[i].current = false; } } - if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("NEW WP SET"); +#else + if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id); +#endif wpm.yaw_reached = false; wpm.pos_reached = false; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); @@ -505,17 +519,29 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Not in list"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); +#endif } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); +#endif } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } break; } @@ -524,7 +550,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_list_t wprl; mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) + if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; @@ -532,8 +558,8 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpm.size > 0) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; wpm.current_wp_id = 0; wpm.current_partner_sysid = msg->sysid; @@ -541,19 +567,19 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); } wpm.current_count = wpm.size; mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); } break; @@ -563,16 +589,37 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_t wpr; mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT WP REQ, state -> SEND"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT 2nd WP REQ"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT 2nd WP REQ"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; wpm.current_wp_id = wpr.seq; @@ -580,32 +627,76 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) + // if (verbose) { - if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); +#endif + break; + } else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + if (wpr.seq != 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: First id != 0"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); +#endif + } } else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); - else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); +#endif + } + else if (wpr.seq >= wpm.size) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP not in list"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); +#endif + } } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: ?"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); +#endif + } } } } else { //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); +#endif } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } } @@ -616,7 +707,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_count_t wpc; mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; @@ -624,8 +715,22 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpc.count > 0) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("WP CMD OK: state -> GETLIST"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("WP CMD OK AGAIN"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); +#endif + } wpm.current_state = MAVLINK_WPM_STATE_GETLIST; wpm.current_wp_id = 0; @@ -633,7 +738,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.current_partner_compid = msg->compid; wpm.current_count = wpc.count; - printf("clearing receive buffer and readying for receiving waypoints\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("CLR RCV BUF: READY"); +#else + if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); +#endif wpm.rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) // { @@ -645,7 +754,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else if (wpc.count == 0) { - printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("COUNT 0"); +#else + if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); +#endif wpm.rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) // { @@ -660,19 +773,48 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CMD"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); +#endif } } else { - if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); - else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); +#endif + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); +#endif + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: ?"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); +#endif + } } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } } @@ -683,18 +825,18 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_t wp; mavlink_msg_waypoint_decode(msg, &wp); - if (verbose) printf("GOT WAYPOINT!"); + // if (verbose) // printf("GOT WAYPOINT!"); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/)) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); @@ -702,11 +844,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.current_wp_id = wp.seq + 1; - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); @@ -730,7 +872,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) if (wpm.waypoints[i].current == 1) { wpm.current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); wpm.yaw_reached = false; wpm.pos_reached = false; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); @@ -761,36 +903,58 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { //we're done receiving waypoints, answer with ack. mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + // printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); } - if (verbose) + // if (verbose) { - if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); + break; + } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + if(!(wp.seq == 0)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); - else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + if (!(wp.seq == wpm.current_wp_id)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + } + else if (!(wp.seq < wpm.current_count)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } } } } else { //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); } - else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) + else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } } break; @@ -801,27 +965,27 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_clear_all_t wpca; mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) { wpm.timestamp_lastaction = now; - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); // Delete all waypoints wpm.size = 0; wpm.current_active_wp_id = -1; wpm.yaw_reached = false; wpm.pos_reached = false; } - else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); } break; } default: { - if (debug) printf("Waypoint: received message of unknown type"); + // if (debug) // printf("Waypoint: received message of unknown type"); break; } } @@ -836,7 +1000,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) if (wpm.timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq); mavlink_wpm_send_waypoint_reached(cur_wp->seq); wpm.timestamp_firstinside_orbit = now; } @@ -867,7 +1031,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.waypoints[wpm.current_active_wp_id].current = true; wpm.pos_reached = false; wpm.yaw_reached = false; - if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); } } } diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h index 4bc32a14a4..2c3416a821 100644 --- a/thirdParty/mavlink/missionlib/waypoints.h +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -47,7 +47,7 @@ enum MAVLINK_WPM_CODES /* WAYPOINT MANAGER - MISSION LIB */ -#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_MAX_WP_COUNT 30 #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates #define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text #define MAVLINK_WPM_SYSTEM_ID 1 @@ -88,4 +88,4 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage* state); -void mavlink_wpm_message_handler(const mavlink_message_t* msg); \ No newline at end of file +void mavlink_wpm_message_handler(const mavlink_message_t* msg); -- GitLab From 9e887c44ac4dacf841ede6f6ae8f28df78848f33 Mon Sep 17 00:00:00 2001 From: LM Date: Fri, 12 Aug 2011 12:28:29 +0200 Subject: [PATCH 019/131] Minor compile fixes --- src/comm/MAVLinkSimulationLink.cc | 2 +- src/uas/UAS.cc | 25 ++++++++++++++++++++----- src/uas/UASWaypointManager.cc | 1 - 3 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 77cc0d1210..db29cc0258 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -409,7 +409,7 @@ void MAVLinkSimulationLink::mainloop() // streampointer += bufferlength; // GLOBAL POSITION - mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed); + mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed, yaw); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3efaebda2d..0b405f608b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -75,7 +75,7 @@ paramsOnceRequested(false), airframe(0), attitudeKnown(false), paramManager(NULL), -attitudeStamped(true), +attitudeStamped(false), lastAttitude(0) { color = UASInterface::getNextColor(); @@ -682,7 +682,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (pos.fix_type > 0) { emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); - emit valueChanged(uasId, "gps speed", "m/s", pos.v, time); + emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time); latitude = pos.lat/(double)1E7; longitude = pos.lon/(double)1E7; altitude = pos.alt/1000.0; @@ -696,12 +696,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); // Smaller than threshold and not NaN - if (pos.v < 1000000 && pos.v == pos.v) { - emit valueChanged(uasId, "speed", "m/s", pos.v, time); + + float vel = pos.vel/100.0f; + + if (vel < 1000000 && !isnan(vel) && !isinf(vel)) { + emit valueChanged(uasId, "speed", "m/s", vel, time); //qDebug() << "GOT GPS RAW"; // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); } else { - emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); } } } @@ -854,6 +857,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } } break; @@ -866,6 +873,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { waypointManager.handleWaypoint(message.sysid, message.compid, &wp); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } } break; @@ -888,6 +899,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } } break; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 5572cded1e..340470c2f0 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project #include "UASWaypointManager.h" #include "UAS.h" #include "mavlink_types.h" -//#include "MainWindow.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout #define PROTOCOL_DELAY_MS 40 ///< minimum delay between sent messages -- GitLab From 1827000b7570ccefffd0b89a1dc6f113d4fc7f9a Mon Sep 17 00:00:00 2001 From: LM Date: Fri, 12 Aug 2011 18:05:39 +0200 Subject: [PATCH 020/131] Deactivated libxbee --- qgroundcontrol.pro | 32 +-- .../mavlinkgen/generator/MAVLinkXMLParser.cc | 233 +++++------------- .../generator/MAVLinkXMLParserV10.h | 1 + src/apps/mavlinkgen/mavlinkgen.pri | 2 - .../mavlinkgen/ui/XMLCommProtocolWidget.cc | 34 +-- .../mavlinkgen/ui/XMLCommProtocolWidget.ui | 45 +--- 6 files changed, 101 insertions(+), 246 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3ce864cecf..f6a8c560ca 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -509,19 +509,19 @@ win32:exists(src/lib/opalrt/OpalApi.h):exists(C:/OPAL-RT/RT-LAB7.2.4/Common/bin) TRANSLATIONS += es-MX.ts \ en-US.ts -# xbee support -# libxbee only supported by linux and windows systems -win32-msvc2008|win32-msvc2010|linux{ - HEADERS += src/comm/XbeeLinkInterface.h \ - src/comm/XbeeLink.h \ - src/ui/XbeeConfigurationWindow.h \ - src/comm/CallConv.h - SOURCES += src/comm/XbeeLink.cpp \ - src/ui/XbeeConfigurationWindow.cpp - DEFINES += XBEELINK - INCLUDEPATH += thirdParty/libxbee -# TO DO: build library when it does not exists already - LIBS += -LthirdParty/libxbee/lib \ - -llibxbee - -} +## xbee support +## libxbee only supported by linux and windows systems +##win32-msvc2008|win32-msvc2010|linux{ +# HEADERS += src/comm/XbeeLinkInterface.h \ +# src/comm/XbeeLink.h \ +# src/ui/XbeeConfigurationWindow.h \ +# src/comm/CallConv.h +# SOURCES += src/comm/XbeeLink.cpp \ +# src/ui/XbeeConfigurationWindow.cpp +# DEFINES += XBEELINK +# INCLUDEPATH += thirdParty/libxbee +## TO DO: build library when it does not exists already +# LIBS += -LthirdParty/libxbee/lib \ +# -llibxbee +# +#} diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc index 66769ae158..34cc9ec9bc 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc @@ -1,24 +1,4 @@ /*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009 - 2011 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - ======================================================================*/ /** @@ -49,8 +29,7 @@ MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QO { doc = new QDomDocument(); QFile file(document); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) - { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { const QString instanceText(QString::fromUtf8(file.readAll())); doc->setContent(instanceText); } @@ -71,8 +50,7 @@ bool MAVLinkXMLParser::generate() bool success = true; // Only generate if output dir is correctly set - if (outputDirName == "") - { + if (outputDirName == "") { emit parseState(tr("ERROR: No output directory given.\nAbort.")); return false; } @@ -118,7 +96,7 @@ bool MAVLinkXMLParser::generate() static int highest_message_id; static int recursion_level; - if (recursion_level == 0){ + if (recursion_level == 0) { highest_message_id = 0; memset(message_lengths, 0, sizeof(message_lengths)); } @@ -135,24 +113,18 @@ bool MAVLinkXMLParser::generate() // Run through root children - while(!n.isNull()) - { + while(!n.isNull()) { // Each child is a message QDomElement e = n.toElement(); // try to convert the node to an element. - if(!e.isNull()) - { - if (e.tagName() == "mavlink") - { + if(!e.isNull()) { + if (e.tagName() == "mavlink") { p = n; n = n.firstChild(); - while (!n.isNull()) - { + while (!n.isNull()) { e = n.toElement(); - if (!e.isNull()) - { + if (!e.isNull()) { // Handle all include tags - if (e.tagName() == "include") - { + if (e.tagName() == "include") { QString incFileName = e.text(); // Load file //QDomDocument includeDoc = QDomDocument(); @@ -162,16 +134,14 @@ bool MAVLinkXMLParser::generate() QFileInfo fInfo(incFileName); QString incFilePath; - if (fInfo.isRelative()) - { + if (fInfo.isRelative()) { QFileInfo rInfo(this->fileName); incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName; pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); } QFile file(incFilePath); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) - { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { emit parseState(QString("Included messages from file: %1").arg(incFileName)); // NEW MODE: CREATE INDIVIDUAL FOLDERS // Create new output directory, parse included XML and generate C-code @@ -205,9 +175,7 @@ bool MAVLinkXMLParser::generate() // } emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); - } - else - { + } else { // Include file could not be opened emit parseState(QString("ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.").arg(fileName)); return false; @@ -215,40 +183,33 @@ bool MAVLinkXMLParser::generate() } // Handle all enum tags - else if (e.tagName() == "version") - { + else if (e.tagName() == "version") { //QString fieldType = e.attribute("type", ""); //QString fieldName = e.attribute("name", ""); QString fieldText = e.text(); // Check if version has been previously set - if (mavlinkVersion != 0) - { + if (mavlinkVersion != 0) { emit parseState(QString("ERROR: Protocol version tag set twice, please use it only once. First version was %1, second version is %2.\nAbort.").arg(mavlinkVersion).arg(fieldText)); return false; } bool ok; int version = fieldText.toInt(&ok); - if (ok && (version > 0) && (version < 256)) - { + if (ok && (version > 0) && (version < 256)) { // Set MAVLink version mavlinkVersion = version; - } - else - { + } else { emit parseState(QString("ERROR: Reading version string failed: %1, string is not an integer number between 1 and 255.\nAbort.").arg(fieldText)); return false; } } // Handle all enum tags - else if (e.tagName() == "enums") - { + else if (e.tagName() == "enums") { // One down into the enums list p = n; n = n.firstChild(); - while (!n.isNull()) - { + while (!n.isNull()) { e = n.toElement(); QString currEnum; @@ -256,25 +217,18 @@ bool MAVLinkXMLParser::generate() // Comment QString comment; - if(!e.isNull() && e.tagName() == "enum") - { + if(!e.isNull() && e.tagName() == "enum") { // Get enum name QString enumName = e.attribute("name", "").toLower(); - if (enumName.size() == 0) - { + if (enumName.size() == 0) { emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); return false; - } - else - { + } else { // Sanity check: Accept only enum names not used previously - if (usedEnumNames->contains(enumName)) - { + if (usedEnumNames->contains(enumName)) { emit parseState(tr("ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(enumName, QString::number(e.lineNumber()), fileName)); return false; - } - else - { + } else { usedEnumNames->insert(enumName, QString::number(e.lineNumber())); } @@ -286,28 +240,22 @@ bool MAVLinkXMLParser::generate() // Get the enum fields QDomNode f = e.firstChild(); - while (!f.isNull()) - { + while (!f.isNull()) { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "entry") - { + if (!e2.isNull() && e2.tagName() == "entry") { QString fieldValue = e2.attribute("value", ""); // If value was given, use it, if not, use the enum iterator // value. The iterator value gets reset by manual values QString fieldName = e2.attribute("name", ""); - if (fieldValue.length() == 0) - { + if (fieldValue.length() == 0) { fieldValue = QString::number(nextEnumValue); nextEnumValue++; - } - else - { + } else { bool ok; nextEnumValue = fieldValue.toInt(&ok) + 1; - if (!ok) - { + if (!ok) { emit parseState(tr("ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.").arg(fieldName, fieldValue)); return false; } @@ -319,8 +267,7 @@ bool MAVLinkXMLParser::generate() { QString sep(" | "); QDomNode pp = e2.firstChild(); - while (!pp.isNull()) - { + while (!pp.isNull()) { QDomElement pp2 = pp.toElement(); if (pp2.isText() || pp2.isCDATASection()) { @@ -359,49 +306,37 @@ bool MAVLinkXMLParser::generate() } // Handle all message tags - else if (e.tagName() == "messages") - { + else if (e.tagName() == "messages") { p = n; n = n.firstChild(); - while (!n.isNull()) - { + while (!n.isNull()) { e = n.toElement(); - if(!e.isNull()) - { + if(!e.isNull()) { //if (e.isNull()) continue; // Get message name QString messageName = e.attribute("name", "").toLower(); - if (messageName.size() == 0) - { + if (messageName.size() == 0) { emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); return false; - } - else - { + } else { // Get message id bool ok; int messageId = e.attribute("id", "-1").toInt(&ok, 10); emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); // Sanity check: Accept only message IDs not used previously - if (usedMessageIDs->contains(messageId)) - { + if (usedMessageIDs->contains(messageId)) { emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); return false; - } - else - { + } else { usedMessageIDs->append(messageId); } // Sanity check: Accept only message names not used previously - if (usedMessageNames->contains(messageName)) - { + if (usedMessageNames->contains(messageName)) { emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); return false; - } - else - { + } else { usedMessageNames->insert(messageName, QString::number(e.lineNumber())); } @@ -440,11 +375,9 @@ bool MAVLinkXMLParser::generate() // Get the message fields QDomNode f = e.firstChild(); - while (!f.isNull()) - { + while (!f.isNull()) { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "field") - { + if (!e2.isNull() && e2.tagName() == "field") { QString fieldType = e2.attribute("type", ""); QString fieldName = e2.attribute("name", ""); QString fieldText = e2.text(); @@ -453,8 +386,7 @@ bool MAVLinkXMLParser::generate() QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); // Send arguments do not work for the version field - if (!fieldType.contains("uint8_t_mavlink_version")) - { + if (!fieldType.contains("uint8_t_mavlink_version")) { // Send arguments are the same for integral types and arrays sendArguments += ", " + fieldName; commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); @@ -462,8 +394,7 @@ bool MAVLinkXMLParser::generate() // MAVLink version field // this is a special field always containing the version define - if (fieldType.contains("uint8_t_mavlink_version")) - { + if (fieldType.contains("uint8_t_mavlink_version")) { // Add field to C structure cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); // Add pack line to message_xx_pack function @@ -473,8 +404,7 @@ bool MAVLinkXMLParser::generate() } // Array handling is different from simple types - else if (fieldType.startsWith("array")) - { + else if (fieldType.startsWith("array")) { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("int8_t*") + " " + fieldName; @@ -487,9 +417,7 @@ bool MAVLinkXMLParser::generate() // Add decode function for this type decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - } - else if (fieldType.startsWith("string")) - { + } else if (fieldType.startsWith("string")) { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("char*") + " " + fieldName; @@ -504,8 +432,7 @@ bool MAVLinkXMLParser::generate() arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); } // Expand array handling to all valid mavlink data types - else if(fieldType.contains('[') && fieldType.contains(']')) - { + else if(fieldType.contains('[') && fieldType.contains(']')) { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + arrayType + "* " + fieldName; @@ -525,8 +452,7 @@ bool MAVLinkXMLParser::generate() // decodeLines += ""; prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); - } - else + } else // Handle simple types like integers and floats { packParameters += ", " + fieldType + " " + fieldName; @@ -544,8 +470,7 @@ bool MAVLinkXMLParser::generate() // message length calculation unsigned element_multiplier = 1; unsigned element_length = 0; - const struct - { + const struct { const char *prefix; unsigned length; } length_map[] = { @@ -562,12 +487,10 @@ bool MAVLinkXMLParser::generate() { "float", 4 }, { "double", 8 }, }; - if (fieldType.contains("[")) - { + if (fieldType.contains("[")) { element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); } - for (unsigned i=0; ipayload%2)[0];").arg("uint8_t", prepends); - } - else if (fieldType == "uint8_t" || fieldType == "int8_t") - { + } else if (fieldType == "uint8_t" || fieldType == "int8_t") { unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); - } - else if (fieldType == "uint16_t" || fieldType == "int16_t") - { + } else if (fieldType == "uint16_t" || fieldType == "int16_t") { unpackingCode = QString("\tgeneric_16bit r;\n\tr.b[1] = (msg->payload%1)[0];\n\tr.b[0] = (msg->payload%1)[1];\n\treturn (%2)r.s;").arg(prepends).arg(fieldType); - } - else if (fieldType == "uint32_t" || fieldType == "int32_t") - { + } else if (fieldType == "uint32_t" || fieldType == "int32_t") { unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.i;").arg(prepends).arg(fieldType); - } - else if (fieldType == "float") - { + } else if (fieldType == "float") { unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.f;").arg(prepends).arg(fieldType); - } - else if (fieldType == "uint64_t" || fieldType == "int64_t") - { + } else if (fieldType == "uint64_t" || fieldType == "int64_t") { unpackingCode = QString("\tgeneric_64bit r;\n\tr.b[7] = (msg->payload%1)[0];\n\tr.b[6] = (msg->payload%1)[1];\n\tr.b[5] = (msg->payload%1)[2];\n\tr.b[4] = (msg->payload%1)[3];\n\tr.b[3] = (msg->payload%1)[4];\n\tr.b[2] = (msg->payload%1)[5];\n\tr.b[1] = (msg->payload%1)[6];\n\tr.b[0] = (msg->payload%1)[7];\n\treturn (%2)r.ll;").arg(prepends).arg(fieldType); - } - else if (fieldType.startsWith("array")) - { + } else if (fieldType.startsWith("array")) { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); - } - else if (fieldType.startsWith("string")) - { + } else if (fieldType.startsWith("string")) { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); } // Generate the message decoding function - if (fieldType.contains("uint8_t_mavlink_version")) - { + if (fieldType.contains("uint8_t_mavlink_version")) { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); decodeLines += ""; prepends += "+sizeof(uint8_t)"; } // Array handling is different from simple types - else if (fieldType.startsWith("array")) - { + else if (fieldType.startsWith("array")) { unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } - else if (fieldType.startsWith("string")) - { + } else if (fieldType.startsWith("string")) { unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } - else if(fieldType.contains('[') && fieldType.contains(']')) - { + } else if(fieldType.contains('[') && fieldType.contains(']')) { // prevent this case from being caught in the following else - } - else - { + } else { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); decodeLines += ""; prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; @@ -653,8 +553,7 @@ bool MAVLinkXMLParser::generate() f = f.nextSibling(); } - if (messageId > highest_message_id) - { + if (messageId > highest_message_id) { highest_message_id = messageId; } message_lengths[messageId] = message_length; @@ -701,8 +600,7 @@ bool MAVLinkXMLParser::generate() mainHeader += "// MESSAGE DEFINITIONS\n\n"; // Create directory if it doesn't exist, report result in success if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName); - for (int i = 0; i < cFiles.size(); i++) - { + for (int i = 0; i < cFiles.size(); i++) { QFile rawFile(dir.filePath(cFiles.at(i).first)); bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text); success = success && ok; @@ -714,8 +612,7 @@ bool MAVLinkXMLParser::generate() mainHeader += "\n\n// MESSAGE LENGTHS\n\n"; mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n"; mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { "; - for (int i=0; i #include #include +#include /** * @brief MAVLink micro air vehicle protocol generator diff --git a/src/apps/mavlinkgen/mavlinkgen.pri b/src/apps/mavlinkgen/mavlinkgen.pri index a5ef920b55..8270518b47 100644 --- a/src/apps/mavlinkgen/mavlinkgen.pri +++ b/src/apps/mavlinkgen/mavlinkgen.pri @@ -29,7 +29,6 @@ FORMS += ui/XMLCommProtocolWidget.ui HEADERS += \ ui/XMLCommProtocolWidget.h \ generator/MAVLinkXMLParser.h \ - generator/MAVLinkXMLParserV10.h \ ui/DomItem.h \ ui/DomModel.h \ ui/QGCMAVLinkTextEdit.h @@ -38,7 +37,6 @@ SOURCES += \ ui/DomItem.cc \ ui/DomModel.cc \ generator/MAVLinkXMLParser.cc \ - generator/MAVLinkXMLParserV10.cc \ ui/QGCMAVLinkTextEdit.cc RESOURCES += mavlinkgen.qrc diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc index 82b37be0ed..337a019581 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc @@ -6,7 +6,6 @@ #include "XMLCommProtocolWidget.h" #include "ui_XMLCommProtocolWidget.h" #include "MAVLinkXMLParser.h" -#include "MAVLinkXMLParserV10.h" #include #include @@ -132,37 +131,18 @@ void XMLCommProtocolWidget::generate() // Syntax check already gives output return; } - - MAVLinkXMLParser* parser = NULL; - MAVLinkXMLParserV10* parserV10 = NULL; - - bool result = false; - - if (m_ui->versionComboBox->currentIndex() == 0) - { - MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); - connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); - result = parser->generate(); - } - else if (m_ui->versionComboBox->currentIndex() == 1) - { - MAVLinkXMLParserV10* parserV10 = new MAVLinkXMLParserV10(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); - connect(parserV10, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); - result = parserV10->generate(); - } - - if (result) - { + + MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + bool result = parser->generate(); + if (result) { QMessageBox msgBox; msgBox.setText(QString("The C code / headers have been generated in folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed())); msgBox.exec(); - } - else - { + } else { QMessageBox::critical(this, tr("C code generation failed, please see the compile log for further information"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok); } - if (parser) delete parser; - if (parserV10) delete parserV10; + delete parser; } void XMLCommProtocolWidget::save() diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui index 878d7b3985..089c8afa76 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui @@ -13,7 +13,7 @@ Form - + 6 @@ -51,12 +51,12 @@ Select input file - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + @@ -97,70 +97,49 @@ Select directory - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + - + Compile Output - + - + No file loaded - + Save file - + Save and generate - + :/images/categories/applications-system.svg:/images/categories/applications-system.svg - - - - Select MAVLink Version - - - - - - - - MAVLink v0.9 (-Aug'10) - - - - - MAVLink v1.0 (Sept'10+) - - - - @@ -171,7 +150,7 @@ - + -- GitLab From 8fef3ca0bd95b4be7d58c2f6f2b357dc95bebb25 Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 12 Aug 2011 22:51:09 +0200 Subject: [PATCH 021/131] Updated embedded MAVLink --- thirdParty/mavlink/include/common/common.h | 21 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../include/common/mavlink_msg_attitude.h | 80 +- .../include/common/mavlink_msg_auth_key.h | 32 +- .../mavlink/include/common/mavlink_msg_boot.h | 32 +- .../mavlink_msg_change_operator_control.h | 56 +- .../mavlink_msg_change_operator_control_ack.h | 48 +- .../include/common/mavlink_msg_cmd_ack.h | 40 +- .../include/common/mavlink_msg_command.h | 90 +- .../include/common/mavlink_msg_command_ack.h | 40 +- .../common/mavlink_msg_control_status.h | 90 +- .../include/common/mavlink_msg_debug.h | 40 +- .../include/common/mavlink_msg_debug_vect.h | 64 +- .../include/common/mavlink_msg_full_state.h | 154 ++-- .../common/mavlink_msg_global_position.h | 80 +- .../common/mavlink_msg_global_position_int.h | 80 +- .../common/mavlink_msg_gps_local_origin_set.h | 48 +- .../include/common/mavlink_msg_gps_raw.h | 104 ++- .../include/common/mavlink_msg_gps_raw_int.h | 104 ++- .../mavlink_msg_gps_set_global_origin.h | 64 +- .../include/common/mavlink_msg_gps_status.h | 72 +- .../include/common/mavlink_msg_heartbeat.h | 48 +- .../common/mavlink_msg_local_position.h | 80 +- .../mavlink_msg_local_position_setpoint.h | 56 +- .../mavlink_msg_local_position_setpoint_set.h | 72 +- .../common/mavlink_msg_manual_control.h | 96 +- .../include/common/mavlink_msg_memory_vect.h | 56 +- .../common/mavlink_msg_named_value_float.h | 40 +- .../common/mavlink_msg_named_value_int.h | 40 +- .../mavlink_msg_nav_controller_output.h | 90 +- .../common/mavlink_msg_param_request_list.h | 40 +- .../common/mavlink_msg_param_request_read.h | 56 +- .../include/common/mavlink_msg_param_set.h | 56 +- .../include/common/mavlink_msg_param_value.h | 56 +- .../mavlink/include/common/mavlink_msg_ping.h | 56 +- .../common/mavlink_msg_position_target.h | 56 +- .../include/common/mavlink_msg_raw_imu.h | 104 ++- .../include/common/mavlink_msg_raw_pressure.h | 64 +- .../common/mavlink_msg_rc_channels_override.h | 104 ++- .../common/mavlink_msg_rc_channels_raw.h | 96 +- .../common/mavlink_msg_rc_channels_scaled.h | 96 +- .../common/mavlink_msg_request_data_stream.h | 64 +- ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 194 ++++ ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 194 ++++ .../common/mavlink_msg_safety_allowed_area.h | 80 +- .../mavlink_msg_safety_set_allowed_area.h | 96 +- .../include/common/mavlink_msg_scaled_imu.h | 104 ++- .../common/mavlink_msg_scaled_pressure.h | 56 +- .../common/mavlink_msg_servo_output_raw.h | 90 +- .../include/common/mavlink_msg_set_altitude.h | 40 +- .../include/common/mavlink_msg_set_mode.h | 40 +- .../include/common/mavlink_msg_set_nav_mode.h | 40 +- ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 212 +++++ .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 212 +++++ .../common/mavlink_msg_state_correction.h | 96 +- .../include/common/mavlink_msg_statustext.h | 40 +- .../include/common/mavlink_msg_sys_status.h | 80 +- .../include/common/mavlink_msg_system_time.h | 32 +- .../common/mavlink_msg_system_time_utc.h | 40 +- .../include/common/mavlink_msg_vfr_hud.h | 72 +- .../include/common/mavlink_msg_waypoint.h | 136 ++- .../include/common/mavlink_msg_waypoint_ack.h | 48 +- .../common/mavlink_msg_waypoint_clear_all.h | 40 +- .../common/mavlink_msg_waypoint_count.h | 48 +- .../common/mavlink_msg_waypoint_current.h | 32 +- .../common/mavlink_msg_waypoint_reached.h | 32 +- .../common/mavlink_msg_waypoint_request.h | 48 +- .../mavlink_msg_waypoint_request_list.h | 40 +- .../common/mavlink_msg_waypoint_set_current.h | 48 +- thirdParty/mavlink/include/mavlink_checksum.h | 339 +++---- thirdParty/mavlink/include/mavlink_data.h | 40 +- thirdParty/mavlink/include/mavlink_options.h | 237 ++--- thirdParty/mavlink/include/mavlink_protocol.h | 852 +++++++++--------- thirdParty/mavlink/include/mavlink_types.h | 229 ++--- .../mavlink/message_definitions/common.xml | 28 +- 75 files changed, 3717 insertions(+), 2965 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_checksum.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_data.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_options.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_protocol.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_types.h diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 8ce7b21f82..2364eb3ecd 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Thursday, August 11 2011, 07:25 UTC + * Generated on Friday, August 12 2011, 20:25 UTC */ #ifndef COMMON_H #define COMMON_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_COMMON @@ -273,12 +273,10 @@ enum MAV_CMD #include "./mavlink_msg_control_status.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" -#include "./mavlink_msg_set_roll_pitch_yaw.h" -#include "./mavlink_msg_set_roll_pitch_yaw_speed.h" -#include "./mavlink_msg_roll_pitch_yaw_setpoint.h" -#include "./mavlink_msg_roll_pitch_yaw_speed_setpoint.h" -#include "./mavlink_msg_attitude_controller_output.h" -#include "./mavlink_msg_position_controller_output.h" +#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" +#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" #include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_position_target.h" #include "./mavlink_msg_state_correction.h" @@ -302,7 +300,12 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 79, 252, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } + +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 7de59d0c68..9c3399f455 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Thursday, August 11 2011, 07:25 UTC + * Generated on Friday, August 12 2011, 20:25 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 8c32cd48c3..c0d0c7350e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -3,16 +3,18 @@ #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 { - 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_attitude_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t us mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index 36366e2dcb..e75ffd1b7e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -3,10 +3,12 @@ #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 { - char key[32]; ///< key + char key[32]; ///< key } mavlink_auth_key_t; #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 @@ -25,7 +27,7 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp 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 + memcpy(p->key, key, sizeof(p->key)); // char[32]:key return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); } @@ -44,7 +46,7 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t 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 + memcpy(p->key, key, sizeof(p->key)); // char[32]:key return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); } @@ -62,23 +64,21 @@ 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 */ - - -#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; - uint16_t checksum; - mavlink_auth_key_t *p = &payload; - memcpy(p->key, key, sizeof(p->key)); // char[32]:key + 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; @@ -89,14 +89,12 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 9051bb199a..0c043e83ab 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -3,10 +3,12 @@ #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 { - uint32_t version; ///< The onboard software version + uint32_t version; ///< The onboard software version } mavlink_boot_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen 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 + p->version = version; // uint32_t:The onboard software version return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BOOT_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_boot_pack_chan(uint8_t system_id, uint8_t com 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 + p->version = version; // uint32_t:The onboard software version return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BOOT_LEN); } @@ -61,23 +63,21 @@ 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; - uint16_t checksum; - mavlink_boot_t *p = &payload; - p->version = version; // uint32_t:The onboard software version + 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; @@ -88,14 +88,12 @@ static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t versio mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 9b1a2e7958..d78480cf57 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -3,13 +3,15 @@ #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 { - 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_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i 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 "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys 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 "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } @@ -77,6 +79,8 @@ 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 @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_change_operator_control_t *p = &payload; - p->target_system = target_system; // uint8_t:System the GCS requests control for - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + 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; @@ -110,14 +110,12 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 8e93156755..2afdd8077a 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 @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst 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 + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t 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 + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_change_operator_control_ack_t *p = &payload; - p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h index b2e7609656..acfc503124 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_CMD_ACK 9 #define MAVLINK_MSG_ID_CMD_ACK_LEN 2 #define MAVLINK_MSG_9_LEN 2 +#define MAVLINK_MSG_ID_CMD_ACK_KEY 0x90 +#define MAVLINK_MSG_9_KEY 0x90 typedef struct __mavlink_cmd_ack_t { - uint8_t cmd; ///< The MAV_CMD ID - uint8_t result; ///< 0: Action DENIED, 1: Action executed + uint8_t cmd; ///< The MAV_CMD ID + uint8_t result; ///< 0: Action DENIED, 1: Action executed } mavlink_cmd_ack_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_cmd_ack_pack(uint8_t system_id, uint8_t compo mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CMD_ACK; - p->cmd = cmd; // uint8_t:The MAV_CMD ID - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_ACK_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_cmd_ack_pack_chan(uint8_t system_id, uint8_t mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CMD_ACK; - p->cmd = cmd; // uint8_t:The MAV_CMD ID - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_ACK_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_cmd_ack_encode(uint8_t system_id, uint8_t com return mavlink_msg_cmd_ack_pack(system_id, component_id, msg, cmd_ack->cmd, cmd_ack->result); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a cmd_ack message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_cmd_ack_encode(uint8_t system_id, uint8_t com * @param cmd The MAV_CMD ID * @param result 0: Action DENIED, 1: Action executed */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_cmd_ack_send(mavlink_channel_t chan, uint8_t cmd, uint8_t result) { mavlink_header_t hdr; mavlink_cmd_ack_t payload; - uint16_t checksum; - mavlink_cmd_ack_t *p = &payload; - p->cmd = cmd; // uint8_t:The MAV_CMD ID - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CMD_ACK_LEN ) + payload.cmd = cmd; // uint8_t:The MAV_CMD ID + payload.result = result; // uint8_t:0: Action DENIED, 1: Action executed hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CMD_ACK_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_cmd_ack_send(mavlink_channel_t chan, uint8_t cmd, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index 7b8afd94b5..72cd1bebcc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_COMMAND 75 #define MAVLINK_MSG_ID_COMMAND_LEN 20 #define MAVLINK_MSG_75_LEN 20 +#define MAVLINK_MSG_ID_COMMAND_KEY 0x14 +#define MAVLINK_MSG_75_KEY 0x14 typedef struct __mavlink_command_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_t; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t compo mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - 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->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_command_pack_chan(uint8_t system_id, uint8_t mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - 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->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LEN); } @@ -96,6 +98,8 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com return mavlink_msg_command_pack(system_id, component_id, msg, command->target_system, command->target_component, command->command, command->confirmation, command->param1, command->param2, command->param3, command->param4); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a command message * @param chan MAVLink channel to send the message @@ -109,24 +113,20 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param3 Parameter 3, as defined by MAV_CMD enum. * @param param4 Parameter 4, as defined by MAV_CMD enum. */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { mavlink_header_t hdr; mavlink_command_t payload; - uint16_t checksum; - mavlink_command_t *p = &payload; - - p->target_system = target_system; // uint8_t:System which should execute the command - p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_COMMAND_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_LEN; @@ -137,14 +137,12 @@ static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t targ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index c7e4a4b0ff..899b12507d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_COMMAND_ACK 76 #define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 #define MAVLINK_MSG_76_LEN 8 +#define MAVLINK_MSG_ID_COMMAND_ACK_KEY 0x16 +#define MAVLINK_MSG_76_KEY 0x16 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c 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 + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint 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 + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_command_ack_t *p = &payload; - p->command = command; // float:Current airspeed in m/s - p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float co mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 8974f07d08..bc19b9c842 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -3,17 +3,19 @@ #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 { - 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; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ 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 + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, u 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 + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } @@ -96,6 +98,8 @@ 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 @@ -109,24 +113,20 @@ 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; - uint16_t checksum; - mavlink_control_status_t *p = &payload; - - p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent - p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled - p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled - p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled - p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + + 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; @@ -137,14 +137,12 @@ static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index 419ef33e84..2de388fc57 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone 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 + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co 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 + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_debug_t *p = &payload; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, f mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 53ef8bd11c..a0565e755a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -3,14 +3,16 @@ #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 { - 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_DEBUG_VECT_FIELD_NAME_LEN 10 @@ -33,11 +35,11 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } @@ -60,11 +62,11 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } @@ -82,6 +84,8 @@ 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 @@ -92,21 +96,17 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param y y * @param z z */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_debug_vect_t *p = &payload; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name - p->usec = usec; // uint64_t:Timestamp - p->x = x; // float:x - p->y = y; // float:y - p->z = z; // float:z + 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; @@ -117,14 +117,12 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h index 263d3e9678..9cacfd9703 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h @@ -3,25 +3,27 @@ #define MAVLINK_MSG_ID_FULL_STATE 67 #define MAVLINK_MSG_ID_FULL_STATE_LEN 56 #define MAVLINK_MSG_67_LEN 56 +#define MAVLINK_MSG_ID_FULL_STATE_KEY 0x26 +#define MAVLINK_MSG_67_KEY 0x26 typedef struct __mavlink_full_state_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) } mavlink_full_state_t; @@ -54,22 +56,22 @@ static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t co mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_FULL_STATE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FULL_STATE_LEN); } @@ -103,22 +105,22 @@ static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8 mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_FULL_STATE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FULL_STATE_LEN); } @@ -136,6 +138,8 @@ static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a full_state message * @param chan MAVLink channel to send the message @@ -157,32 +161,28 @@ static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t * @param yacc Y acceleration (mg) * @param zacc Z acceleration (mg) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) { mavlink_header_t hdr; mavlink_full_state_t payload; - uint16_t checksum; - mavlink_full_state_t *p = &payload; - - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_FULL_STATE_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) + 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_FULL_STATE_LEN; @@ -193,14 +193,12 @@ static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0x26, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index 26e2d8a133..c3a1accfa8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -3,16 +3,18 @@ #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 { - 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_global_position_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) - p->lat = lat; // float:Latitude, in degrees - p->lon = lon; // float:Longitude, in degrees - p->alt = alt; // float:Absolute altitude, in meters - p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) - p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) - p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 9dbc636e20..218a757c27 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -3,16 +3,18 @@ #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 { - 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u 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 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ 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 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_global_position_int_t *p = &payload; - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 831fc69689..8303f350f9 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 @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, 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 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack_chan(uint8_t system 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 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_gps_local_origin_set_t *p = &payload; - p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 - p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 - p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 0ee5e0f3f7..ca4f86e371 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -3,19 +3,21 @@ #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 { - 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; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo 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 + 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); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t 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 + 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); } @@ -106,6 +108,8 @@ 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 @@ -121,26 +125,22 @@ 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; - uint16_t checksum; - mavlink_gps_raw_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // float:Latitude in degrees - p->lon = lon; // float:Longitude in degrees - p->alt = alt; // float:Altitude in meters - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + 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; @@ -151,14 +151,12 @@ static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t use mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 c0cf3bba93..bc281fe0ea 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -3,19 +3,21 @@ #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 { - 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; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c 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 + 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); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint 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 + 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); } @@ -106,6 +108,8 @@ 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 @@ -121,26 +125,22 @@ 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; - uint16_t checksum; - mavlink_gps_raw_int_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // int32_t:Latitude in 1E7 degrees - p->lon = lon; // int32_t:Longitude in 1E7 degrees - p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL - p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 + 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; @@ -151,14 +151,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 bccfdba02c..edb2b8d543 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 @@ -3,14 +3,16 @@ #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 { - 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; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t syste 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } @@ -81,6 +83,8 @@ 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 @@ -91,21 +95,17 @@ 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; - uint16_t checksum; - mavlink_gps_set_global_origin_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->latitude = latitude; // int32_t:global position * 1E7 - p->longitude = longitude; // int32_t:global position * 1E7 - p->altitude = altitude; // int32_t:global position * 1000 + 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; @@ -116,14 +116,12 @@ static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index a974432d6d..450a98ccdb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -3,15 +3,17 @@ #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 { - 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_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 @@ -39,12 +41,12 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co 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 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); } @@ -68,12 +70,12 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 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 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); } @@ -91,6 +93,8 @@ 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 @@ -102,22 +106,18 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. * @param satellite_snr Signal to noise ratio of satellite */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const 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; - uint16_t checksum; - mavlink_gps_status_t *p = &payload; - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID - memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization - memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite - memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. - memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + 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; @@ -128,14 +128,12 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 7c686831ee..466f8a783d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_HEARTBEAT 0 #define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 #define MAVLINK_MSG_0_LEN 3 +#define MAVLINK_MSG_ID_HEARTBEAT_KEY 0x47 +#define MAVLINK_MSG_0_KEY 0x47 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; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - 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; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + uint8_t mavlink_version; ///< MAVLink version } mavlink_heartbeat_t; @@ -27,10 +29,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com 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:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + 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); } @@ -49,10 +51,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ 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:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + 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); } @@ -69,6 +71,8 @@ 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); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -76,20 +80,16 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; mavlink_heartbeat_t payload; - uint16_t checksum; - mavlink_heartbeat_t *p = &payload; - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + 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:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + 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; @@ -99,14 +99,12 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0x47, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index fb6648714a..280469009f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -3,16 +3,18 @@ #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 { - 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, u 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_local_position_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - p->vx = vx; // float:X Speed - p->vy = vy; // float:Y Speed - p->vz = vz; // float:Z Speed + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint6 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 8ea4ca458c..6fd3bb0435 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -3,13 +3,15 @@ #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 { - 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_local_position_setpoint_t *p = &payload; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 d7aa0d5bc0..c26258c5ba 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 @@ -3,15 +3,17 @@ #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 { - 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack_chan(uint8_t 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_local_position_setpoint_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index eeb05487af..a8cecbec5d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -3,18 +3,20 @@ #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 { - 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; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ 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 + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u 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 + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_manual_control_t *p = &payload; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h index ce90911d16..22857f9fee 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -3,13 +3,15 @@ #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 { - 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_MEMORY_VECT_FIELD_VALUE_LEN 32 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c 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 + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint 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 + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); } @@ -77,6 +79,8 @@ 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 @@ -86,20 +90,16 @@ 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 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) { mavlink_header_t hdr; mavlink_memory_vect_t payload; - uint16_t checksum; - mavlink_memory_vect_t *p = &payload; - p->address = address; // uint16_t:Starting address of the debug variables - p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + 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; @@ -110,14 +110,12 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 ce5c680ad2..6775ea32d1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -3,11 +3,13 @@ #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 { - 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_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } @@ -67,6 +69,8 @@ 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 @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param name Name of the debug variable * @param value Floating point value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_named_value_float_t *p = &payload; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value + 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; @@ -96,14 +96,12 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, co mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 1c75a5be16..86789129e0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -3,11 +3,13 @@ #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 { - 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_NAMED_VALUE_INT_FIELD_NAME_LEN 10 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } @@ -67,6 +69,8 @@ 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 @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param name Name of the debug variable * @param value Signed integer value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_named_value_int_t *p = &payload; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value + 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; @@ -96,14 +96,12 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, cons mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 ef878a3f7c..351fa5205f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -3,17 +3,19 @@ #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 { - 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; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, 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 + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste 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 + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } @@ -96,6 +98,8 @@ 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 @@ -109,24 +113,20 @@ 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; - uint16_t checksum; - mavlink_nav_controller_output_t *p = &payload; - - p->nav_roll = nav_roll; // float:Current desired roll in degrees - p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees - p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees - p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees - p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters - p->alt_error = alt_error; // float:Current altitude error in meters - p->aspd_error = aspd_error; // float:Current airspeed error in meters/second - p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + + 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; @@ -137,14 +137,12 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 74ed51442e..28fb9c4146 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_param_request_list_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 86c0a4b1d3..614fc43282 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -3,13 +3,15 @@ #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 { - 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_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } @@ -77,6 +79,8 @@ 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 @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_id Onboard parameter id * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { mavlink_header_t hdr; mavlink_param_request_read_t payload; - uint16_t checksum; - mavlink_param_request_read_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + 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; @@ -110,14 +110,12 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index 7dfb5b6dcc..79e508be30 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PARAM_SET 23 #define MAVLINK_MSG_ID_PARAM_SET_LEN 22 #define MAVLINK_MSG_23_LEN 22 +#define MAVLINK_MSG_ID_PARAM_SET_KEY 0x2D +#define MAVLINK_MSG_23_KEY 0x2D 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 + 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 } mavlink_param_set_t; #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com 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->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ 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->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); } @@ -77,6 +79,8 @@ 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); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_set message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_id Onboard parameter id * @param param_value Onboard parameter value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { mavlink_header_t hdr; mavlink_param_set_t payload; - uint16_t checksum; - mavlink_param_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value + 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 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PARAM_SET_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0x2D, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 19f618ab44..877a904846 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PARAM_VALUE 22 #define MAVLINK_MSG_ID_PARAM_VALUE_LEN 24 #define MAVLINK_MSG_22_LEN 24 +#define MAVLINK_MSG_ID_PARAM_VALUE_KEY 0xA3 +#define MAVLINK_MSG_22_KEY 0xA3 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 + float param_value; ///< Onboard parameter value + uint16_t param_count; ///< Total number of onboard parameters + uint16_t param_index; ///< Index of this onboard parameter + char param_id[16]; ///< Onboard parameter id } mavlink_param_value_t; #define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c 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_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint 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_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } @@ -77,6 +79,8 @@ 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_count, param_value->param_index); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_value message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_count Total number of onboard parameters * @param param_index Index of this onboard parameter */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_header_t hdr; mavlink_param_value_t payload; - uint16_t checksum; - mavlink_param_value_t *p = &payload; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + 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_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; @@ -110,14 +110,12 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index f22a195c79..7cf69d57cc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PING 3 #define MAVLINK_MSG_ID_PING_LEN 14 #define MAVLINK_MSG_3_LEN 14 +#define MAVLINK_MSG_ID_PING_KEY 0xE2 +#define MAVLINK_MSG_3_KEY 0xE2 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen 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 + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com 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 + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_ping_t *p = &payload; - p->seq = seq; // uint32_t:PING sequence - p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - p->time = time; // uint64_t:Unix timestamp in microseconds + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index 1a65ed0a40..113055c3d2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -3,13 +3,15 @@ #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 { - 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_position_target_t *p = &payload; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, floa mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index aa0f306727..ad3acf0584 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -3,19 +3,21 @@ #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 { - 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; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo 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) + 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); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t 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) + 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); } @@ -106,6 +108,8 @@ 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 @@ -121,26 +125,22 @@ 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; - uint16_t checksum; - mavlink_raw_imu_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (raw) - p->yacc = yacc; // int16_t:Y acceleration (raw) - p->zacc = zacc; // int16_t:Z acceleration (raw) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) - p->xmag = xmag; // int16_t:X Magnetic field (raw) - p->ymag = ymag; // int16_t:Y Magnetic field (raw) - p->zmag = zmag; // int16_t:Z Magnetic field (raw) + 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; @@ -151,14 +151,12 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t use mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 4d3a9ba258..3ae58d82cf 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -3,14 +3,16 @@ #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 { - 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; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } @@ -81,6 +83,8 @@ 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 @@ -91,21 +95,17 @@ 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; - uint16_t checksum; - mavlink_raw_pressure_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // int16_t:Absolute pressure (raw) - p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) - p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) - p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + 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; @@ -116,14 +116,12 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 b9dfe85e96..1ec726f9df 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -3,19 +3,21 @@ #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 { - 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; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, 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 + 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); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system 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 + 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); } @@ -106,6 +108,8 @@ 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 @@ -121,26 +125,22 @@ 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; - uint16_t checksum; - mavlink_rc_channels_override_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + 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; @@ -151,14 +151,12 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 56de4d83c8..840cfbf2cb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -3,18 +3,20 @@ #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 { - 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; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 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% + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, 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% + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_rc_channels_raw_t *p = &payload; - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 fd2e57ad4c..95ba39510e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -3,18 +3,20 @@ #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 { - 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; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui 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% + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i 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% + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_rc_channels_scaled_t *p = &payload; - p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, i mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 b779e267e8..0c6dd6562e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -3,14 +3,16 @@ #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 { - uint16_t req_message_rate; ///< The requested interval between two messages of this type - uint8_t target_system; ///< The target requested to send the message stream. - uint8_t target_component; ///< The target requested to send the message stream. - uint8_t req_stream_id; ///< The ID of the requested message type - 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 message type + uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u 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 message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ 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 message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } @@ -81,6 +83,8 @@ 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 @@ -91,21 +95,17 @@ 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; - uint16_t checksum; - mavlink_request_data_stream_t *p = &payload; - p->target_system = target_system; // uint8_t:The target requested to send the message stream. - p->target_component = target_component; // uint8_t:The target requested to send the message stream. - p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + 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 message type + 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; @@ -116,14 +116,12 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 new file mode 100644 index 0000000000..a9f7231f4c --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -0,0 +1,194 @@ +// 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 +{ + 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; + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_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 + * @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) +{ + 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 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_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 + * @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) +{ + 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 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_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 + * + * @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_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 ); + + 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 +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +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); +} + +/** + * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + memcpy( roll_pitch_yaw_speed_thrust_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h new file mode 100644 index 0000000000..fb00bec610 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -0,0 +1,194 @@ +// 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 +{ + 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; + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_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 + * @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) +{ + 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 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_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 + * @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) +{ + 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 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_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 + * + * @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_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 ); + + 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 +} + +#endif +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_thrust_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +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); +} + +/** + * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + memcpy( roll_pitch_yaw_thrust_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_thrust_setpoint_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h index a660e8bed5..899d12bcb7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -3,16 +3,18 @@ #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 { - 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u 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 + 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_ALLOWED_AREA_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ 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 + 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_ALLOWED_AREA_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_safety_allowed_area_t *p = &payload; - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 9e8ef1e44b..b7afcfd28f 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 @@ -3,18 +3,20 @@ #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 { - 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; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i 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 + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys 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 + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_safety_set_allowed_area_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 8f381f22c1..e6450010b7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -3,19 +3,21 @@ #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 { - 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; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co 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) + 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); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 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) + 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); } @@ -106,6 +108,8 @@ 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 @@ -121,26 +125,22 @@ 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; - uint16_t checksum; - mavlink_scaled_imu_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) - p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) - p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) - p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + 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; @@ -151,14 +151,12 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 39d29440f2..59b1dcc62c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -3,13 +3,15 @@ #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 { - 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_scaled_pressure_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // float:Absolute pressure (hectopascal) - p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) - p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 5aa3cc8d0d..976506d561 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -3,17 +3,19 @@ #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 { - 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; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint 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 + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, 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 + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } @@ -96,6 +98,8 @@ 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 @@ -109,24 +113,20 @@ 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; - uint16_t checksum; - mavlink_servo_output_raw_t *p = &payload; - - p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds - p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds - p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds - p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds - p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds - p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds - p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds - p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + + 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; @@ -137,14 +137,12 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 9b50027f1d..a4fea868c0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t 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 + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_set_altitude_pack_chan(uint8_t system_id, uin 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 + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_set_altitude_t *p = &payload; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index ad796f1e0e..85e2d4b9f7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp 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 + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t 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 + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_set_mode_t *p = &payload; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 1147440097..f42432e3a8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t 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 + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack_chan(uint8_t system_id, uin 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 + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_set_nav_mode_t *p = &payload; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 new file mode 100644 index 0000000000..e2d6e9afc0 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -0,0 +1,212 @@ +// 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 +{ + 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; + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + 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 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + 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 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief Send a set_roll_pitch_yaw_speed_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +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 ); + + 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 +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) +{ + 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); +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + memcpy( set_roll_pitch_yaw_speed_thrust, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_thrust_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h new file mode 100644 index 0000000000..5d8e5f8eae --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -0,0 +1,212 @@ +// 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 +{ + 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; + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + 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 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + 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 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw_thrust struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief Send a set_roll_pitch_yaw_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +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 ); + + 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 +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll from set_roll_pitch_yaw_thrust message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw_thrust message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw_thrust message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a set_roll_pitch_yaw_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + memcpy( set_roll_pitch_yaw_thrust, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_thrust_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index f101513eb8..f92a132653 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -3,18 +3,20 @@ #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 { - 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; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint 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 + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, 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 + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_state_correction_t *p = &payload; - p->xErr = xErr; // float:x position error - p->yErr = yErr; // float:y position error - p->zErr = zErr; // float:z position error - p->rollErr = rollErr; // float:roll error (radians) - p->pitchErr = pitchErr; // float:pitch error (radians) - p->yawErr = yawErr; // float:yaw error (radians) - p->vxErr = vxErr; // float:x velocity - p->vyErr = vyErr; // float:y velocity - p->vzErr = vzErr; // float:z velocity + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index e53bc5390e..beb411483f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -3,11 +3,13 @@ #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 { - 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_STATUSTEXT_FIELD_TEXT_LEN 50 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co 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 + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 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 + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -67,6 +69,8 @@ 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 @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param severity Severity of status, 0 = info message, 255 = critical fault * @param text Status text message, without null termination character */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) { mavlink_header_t hdr; mavlink_statustext_t payload; - uint16_t checksum; - mavlink_statustext_t *p = &payload; - p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + 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; @@ -96,14 +96,12 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index 1ca8cec2ec..503cf19a04 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_SYS_STATUS 34 #define MAVLINK_MSG_ID_SYS_STATUS_LEN 11 #define MAVLINK_MSG_34_LEN 11 +#define MAVLINK_MSG_ID_SYS_STATUS_KEY 0x6F +#define MAVLINK_MSG_34_KEY 0x6F typedef struct __mavlink_sys_status_t { - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) - uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) - uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) - uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - uint8_t status; ///< System status flag, see MAV_STATUS ENUM + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) + uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) + uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) + uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + uint8_t status; ///< System status flag, see MAV_STATUS ENUM } mavlink_sys_status_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); } @@ -91,6 +93,8 @@ 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->mode, sys_status->nav_mode, sys_status->status, sys_status->load, sys_status->vbat, sys_status->battery_remaining, sys_status->packet_drop); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a sys_status message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_header_t hdr; mavlink_sys_status_t payload; - uint16_t checksum; - mavlink_sys_status_t *p = &payload; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYS_STATUS_LEN ) + 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.load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + payload.vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + payload.battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + payload.packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t m mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0x6F, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index c2328a68fc..790be507ab 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_SYSTEM_TIME 2 #define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 8 #define MAVLINK_MSG_2_LEN 8 +#define MAVLINK_MSG_ID_SYSTEM_TIME_KEY 0xE8 +#define MAVLINK_MSG_2_KEY 0xE8 typedef struct __mavlink_system_time_t { - uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. + uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. } mavlink_system_time_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c 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_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint 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_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } @@ -61,23 +63,21 @@ 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); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a system_time message * @param chan MAVLink channel to send the message * * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { mavlink_header_t hdr; mavlink_system_time_t payload; - uint16_t checksum; - mavlink_system_time_t *p = &payload; - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + 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. hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; @@ -88,14 +88,12 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0xE8, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 414b4984f0..5ee9fcb9d4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 #define MAVLINK_MSG_4_LEN 8 +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_KEY 0x4C +#define MAVLINK_MSG_4_KEY 0x4C 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 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 + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, 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 + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_system_time_utc_t *p = &payload; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index 0b757cef66..abb6fdcf17 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -3,15 +3,17 @@ #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 { - 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo 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 + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t 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 + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_vfr_hud_t *p = &payload; - p->airspeed = airspeed; // float:Current airspeed in m/s - p->groundspeed = groundspeed; // float:Current ground speed in m/s - p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) - p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 - p->alt = alt; // float:Current altitude (MSL), in meters - p->climb = climb; // float:Current climb rate in meters/second + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index a29a26b0ec..71ac9f7f3b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -3,23 +3,25 @@ #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 { - 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; @@ -50,20 +52,20 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp 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 + 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); } @@ -95,20 +97,20 @@ static inline uint16_t mavlink_msg_waypoint_pack_chan(uint8_t system_id, uint8_t 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 + 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); } @@ -126,6 +128,8 @@ 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 @@ -145,30 +149,26 @@ 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; - uint16_t checksum; - mavlink_waypoint_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - p->current = current; // uint8_t:false:0, true:1 - p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp - p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - p->x = x; // float:PARAM5 / local: x position, global: latitude - p->y = y; // float:PARAM6 / y position: global: longitude - p->z = z; // float:PARAM7 / z position: global: altitude + 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; @@ -179,14 +179,12 @@ static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t tar mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 8a5d52ed38..2ea2aae694 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack_chan(uint8_t system_id, uin 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_waypoint_ack_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->type = type; // uint8_t:0: OK, 1: Error + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 b96a80696a..8e7a1a9c21 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack_chan(uint8_t system_i 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_waypoint_clear_all_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 8856c3c445..19cea81698 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_count_pack_chan(uint8_t system_id, u 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_waypoint_count_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->count = count; // uint16_t:Number of Waypoints in the Sequence + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 9c75639c79..b3ed6ce316 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -3,10 +3,12 @@ #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 { - uint16_t seq; ///< Sequence + uint16_t seq; ///< Sequence } mavlink_waypoint_current_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint 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 + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, 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 + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } @@ -61,23 +63,21 @@ 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; - uint16_t checksum; - mavlink_waypoint_current_t *p = &payload; - p->seq = seq; // uint16_t:Sequence + 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; @@ -88,14 +88,12 @@ static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 5e1f144e8b..8014161914 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -3,10 +3,12 @@ #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 { - uint16_t seq; ///< Sequence + uint16_t seq; ///< Sequence } mavlink_waypoint_reached_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint 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 + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, 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 + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } @@ -61,23 +63,21 @@ 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; - uint16_t checksum; - mavlink_waypoint_reached_t *p = &payload; - p->seq = seq; // uint16_t:Sequence + 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; @@ -88,14 +88,12 @@ static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index 9f3ccdbe31..da321fc17a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_request_pack_chan(uint8_t system_id, 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_waypoint_request_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 34d8276d07..44efd58f42 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack_chan(uint8_t syste 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_waypoint_request_list_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 829f29982b..994a0dc6d3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack_chan(uint8_t system 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_waypoint_set_current_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/mavlink_checksum.h b/thirdParty/mavlink/include/mavlink_checksum.h old mode 100755 new mode 100644 index 0f42251741..fdcee99d16 --- a/thirdParty/mavlink/include/mavlink_checksum.h +++ b/thirdParty/mavlink/include/mavlink_checksum.h @@ -1,156 +1,183 @@ -/** @file - * @brief MAVLink comm protocol checksum routines. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _CHECKSUM_H_ -#define _CHECKSUM_H_ - -#include "inttypes.h" - - -/** - * - * CALCULATE THE CHECKSUM - * - */ - -#define X25_INIT_CRC 0xffff -#define X25_VALIDATE_CRC 0xf0b8 - -/** - * @brief Accumulate the X.25 CRC by adding one char at a time. - * - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new char to hash - * @param crcAccum the already accumulated checksum - **/ -static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) -{ - /*Accumulate one byte of data into the CRC*/ - uint8_t tmp; - - tmp=data ^ (uint8_t)(*crcAccum &0xff); - tmp^= (tmp<<4); - *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -// *crcAccum += data; // super simple to test -} - -/** - * @brief Initiliaze the buffer for the X.25 CRC - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init(uint16_t* crcAccum) -{ - *crcAccum = X25_INIT_CRC; -} - -/** - * @brief Initiliaze the buffer for the X.25 CRC to a specified value - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) -{ - *crcAccum = crcValue; -} - - - -/** - * @brief Calculates the X.25 checksum on a byte buffer - * - * @param pBuffer buffer containing the byte array to hash - * @param length length of the byte array - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) -{ - - // For a "message" of length bytes contained in the unsigned char array - // pointed to by pBuffer, calculate the CRC - // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed - - uint16_t crcTmp; - //uint16_t tmp; - uint8_t* pTmp; - int i; - - pTmp=pBuffer; - - - /* init crcTmp */ - crc_init(&crcTmp); - - for (i = 0; i < length; i++){ - crc_accumulate(*pTmp++, &crcTmp); - } - - /* This is currently not needed, as only the checksum over payload should be computed - tmp = crcTmp; - crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); - crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); - *checkConst = tmp; - */ - return(crcTmp); -} - - -/** - * @brief Calculates the X.25 checksum on a msg buffer - * - * @param pMSG buffer containing the msg to hash - * @param length number of bytes to hash - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) -{ - - // For a "message" of length bytes contained in the unsigned char array - // pointed to by pBuffer, calculate the CRC - // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed - - uint16_t crcTmp; - //uint16_t tmp; - uint8_t* pTmp; - int i; - - pTmp=&pMSG->len; - - /* init crcTmp */ - crc_init(&crcTmp); - - for (i = 0; i < 5; i++){ - crc_accumulate(*pTmp++, &crcTmp); - } - - pTmp=&pMSG->payload[0]; - for (; i < length; i++){ - crc_accumulate(*pTmp++, &crcTmp); - } - - /* This is currently not needed, as only the checksum over payload should be computed - tmp = crcTmp; - crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); - crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); - *checkConst = tmp; - */ - return(crcTmp); -} - - - - -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus -} -#endif +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_mem(uint8_t *pBuffer, uint16_t *crcTmp, 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 tmp; + //uint8_t* pTmp; + int i; + +// pTmp=pBuffer; + + for (i = 0; i < length; i++){ + crc_accumulate(*pBuffer++, crcTmp); + } + + return(*crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_data.h b/thirdParty/mavlink/include/mavlink_data.h old mode 100755 new mode 100644 index ce51e2b93a..17c88cc2a9 --- a/thirdParty/mavlink/include/mavlink_data.h +++ b/thirdParty/mavlink/include/mavlink_data.h @@ -1,21 +1,21 @@ -/** @file - * @brief Main MAVLink comm protocol data. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifndef _ML_DATA_H_ -#define _ML_DATA_H_ - -#include "mavlink_types.h" - -#ifdef MAVLINK_CHECK_LENGTH -const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -#endif - -const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; - -mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +mavlink_system_t mavlink_system; #endif \ No newline at end of file diff --git a/thirdParty/mavlink/include/mavlink_options.h b/thirdParty/mavlink/include/mavlink_options.h old mode 100755 new mode 100644 index bc337f6176..550a85ae55 --- a/thirdParty/mavlink/include/mavlink_options.h +++ b/thirdParty/mavlink/include/mavlink_options.h @@ -1,102 +1,135 @@ -/** @file - * @brief MAVLink comm protocol option constants. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _ML_OPTIONS_H_ -#define _ML_OPTIONS_H_ - - -/** - * - * Receive message length check option. On receive verify the length field - * as soon as the message ID field is received. Requires a 256 byte const - * table. Comment out the define to leave out the table and code to check it. - * - */ -//#define MAVLINK_CHECK_LENGTH - -/** - * - * Receive message buffer option. This option should be used only when the - * side effects are understood but allows the underlying program access to - * the internal recieve buffer - eliminating the usual double buffering. It - * also REQUIRES changes in the return type of mavlink_parse_char so only - * enable if you make the changes required. Default DISABLED. - * - */ -//#define MAVLINK_STATIC_BUFFER - -/** - * - * Receive message buffers option. This option defines how many msg buffers - * mavlink will define, and thereby how many links it can support. A default - * will be supplied if the symbol is not pre-defined, dependant on the make - * envionment. The default is 16 for a recognised OS envionment and 1 - * otherwise. - * - */ -#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) -#undef MAVLINK_COMM_NB - #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) - #define MAVLINK_COMM_NB 16 - #else - #define MAVLINK_COMM_NB 1 - #endif -#endif - - -/** - * - * Data relization option. This option controls inclusion of the file - * mavlink_data.h in the current compile unit - thus defining mavlink's - * variables. Default is ON (not defined) because typically mavlink.h is only - * included once in a system but if it was used in two files there would - * be duplicate variables at link time. Normal practice would be to define - * this symbol outside of this file as defining it here will cause missing - * symbols at link time. In other words in the first file to include mavlink.h - * do not define this sybol, then define this symbol in all other files before - * including mavlink.h - * - */ -//#define MAVLINK_NO_DATA -#ifdef MAVLINK_NO_DATA - #undef MAVLINK_DATA -#else - #define MAVLINK_DATA -#endif - -/** - * - * Custom data const data relization and access options. - * This define is placed in the form - * const uint8_t MAVLINK_CONST name[] = { ... }; - * for the keys table and (if applicable) lengths table to tell the compiler - * were to put the data. The access option is placed in the form - * variable = MAVLINK_CONST_READ( name[i] ); - * in order to allow custom read function's or accessors. - * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as - * MAVLINK_CONST_READ( a ) a - * These symbols are only defined if not already defined allowing this file - * to remain unchanged while the actual definitions are maintained in external - * files. - * - */ -#ifndef MAVLINK_CONST -#define MAVLINK_CONST -#endif -#ifndef MAVLINK_CONST_READ -#define MAVLINK_CONST_READ( a ) a -#endif - - -#endif /* _ML_OPTIONS_H_ */ - -#ifdef __cplusplus -} -#endif +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +//#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STATIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +/** + * + * Convience functions. These are all in one send functions that are very + * easy to use. Just define the symbol MAVLINK_USE_CONVENIENCE_FUNCTIONS. + * These functions also support a buffer check, to ensure there is enough + * space in your comm buffer that the function would not block - it could + * also be used as the basis of a MUTEX. This is implemented in the send + * function as a macro with two arguments, first the comm chan number and + * the message length in the form + * MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LEN ) + * followed by the function code and then + * MAVLINK_BUFFER_CHECK_START + * Note that there are no terminators on these statements to allow for + * code nesting or other constructs. Default value for both is empty. + * A sugested implementation is shown below and the symbols will be defined + * only if they are not allready. + * + * if ( serial_space( chan ) > len ) { // serial_space returns available space + * ..... code that creates message + * } + * + * #define MAVLINK_BUFFER_CHECK_START( c, l ) if ( serial_space( c ) > l ) { + * #define MAVLINK_BUFFER_CHECK_END } + * + */ +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +#ifndef MAVLINK_BUFFER_CHECK_START +#define MAVLINK_BUFFER_CHECK_START( c, l ) ; +#endif +#ifndef MAVLINK_BUFFER_CHECK_END +#define MAVLINK_BUFFER_CHECK_END ; +#endif + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h old mode 100755 new mode 100644 index fe63af913e..596b5ffcf1 --- a/thirdParty/mavlink/include/mavlink_protocol.h +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -1,424 +1,428 @@ -/** @file - * @brief Main MAVLink comm protocol routines. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifndef _MAVLINK_PROTOCOL_H_ -#define _MAVLINK_PROTOCOL_H_ - -#include "mavlink_types.h" - -#include "mavlink_checksum.h" - -#ifdef MAVLINK_CHECK_LENGTH -extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; -#endif - -extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; - -extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - - -/** - * @brief Initialize the communication stack - * - * This function has to be called before using commParseBuffer() to initialize the different status registers. - * - * @return Will initialize the different buffers and status registers. - */ -static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) -{ - if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) - { - initStatus->ck_a = 0; - initStatus->ck_b = 0; - initStatus->msg_received = 0; - initStatus->buffer_overrun = 0; - initStatus->parse_error = 0; - initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; - initStatus->packet_idx = 0; - initStatus->packet_rx_drop_count = 0; - initStatus->packet_rx_success_count = 0; - initStatus->current_rx_seq = 0; - initStatus->current_tx_seq = 0; - } -} - -static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ - - return &m_mavlink_status[chan]; -} - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. - * - * @warning This function implicitely assumes the message is sent over channel zero. - * if the message is sent over a different channel it will reach the receiver - * without error, BUT the sequence number might be wrong due to the wrong - * channel sequence counter. This will result is wrongly reported excessive - * packet loss. Please use @see mavlink_{pack|encode}_headerless and then - * @see mavlink_finalize_message_chan before sending for a correct channel - * assignment. Please note that the mavlink_msg_xxx_pack and encode functions - * assign channel zero as default and thus induce possible loss counter errors.\ - * They have been left to ensure code compatibility. - * - * @see mavlink_finalize_message_chan - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length, usually just the counter incremented while packing the message - */ -static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; -// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte - msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; -} - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length, usually just the counter incremented while packing the message - */ -static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; -// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte - msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; -} - -/** - * @brief Pack a message to send it over a serial byte stream - */ -static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) -{ - *(buffer+0) = MAVLINK_STX; ///< Start transmit -// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload - memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header - memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; -// return 0; -} - -/** - * @brief Get the required buffer size for this message - */ -static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) -{ - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - -union checksum_ { - uint16_t s; - uint8_t c[2]; -}; - -static inline void mavlink_start_checksum(mavlink_message_t* msg) -{ - union checksum_ ck; - crc_init(&(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; -} - -static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - union checksum_ ck; - ck.c[0] = msg->ck_a; - ck.c[1] = msg->ck_b; - crc_accumulate(c, &(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -#ifdef MAVLINK_STATIC_BUFFER -static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -#else -static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -#endif -{ - // Initializes only once, values keep unchanged after first initialization - mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); - - mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); -#ifdef MAVLINK_CHECK_LENGTH - if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) - status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway - else ; -#endif - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - rxmsg->payload[status->packet_idx++] = c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - mavlink_update_checksum(rxmsg, - MAVLINK_CONST_READ( mavlink_msg_lengths[rxmsg->msgid] ) ); - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: - if (c != rxmsg->ck_a) - { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != rxmsg->ck_b) - {// Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if ( r_message != NULL ) - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - else ; - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - if (status->msg_received == 1) - { - if ( r_message != NULL ) - return r_message; - else return rxmsg; - } else return NULL; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define a similar function - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - - -static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) -{ - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - comm_send_ch(chan, MAVLINK_STX); - comm_send_ch(chan, msg->len); - comm_send_ch(chan, msg->seq); - comm_send_ch(chan, msg->sysid); - comm_send_ch(chan, msg->compid); - comm_send_ch(chan, msg->msgid); - for(uint16_t i = 0; i < msg->len; i++) - { - comm_send_ch(chan, msg->payload[i]); - } - comm_send_ch(chan, msg->ck_a); - comm_send_ch(chan, msg->ck_b); -} - -static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) -{ - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - for(uint16_t i = 0; i < num; i++) - { - comm_send_ch( chan, mem[i] ); - } -} - */ -static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); -static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); -#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) -#endif - -#endif /* _MAVLINK_PROTOCOL_H_ */ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +extern mavlink_system_t mavlink_system; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + 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; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + 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; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit + // memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + // return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->ck); +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->ck); +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STATIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + { + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + break; + } else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; +#ifdef MAVLINK_STATIC_BUFFER + if (status->msg_received == 1) + { + if ( r_message != NULL ) + { + return r_message; + } + else + { + return rxmsg; + } + } else return NULL; +#else + if (status->msg_received == 1) + return 1; + else return 0; +#endif +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, uint8_t *mem, uint16_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h old mode 100755 new mode 100644 index 36d8d57c5d..77b2e24096 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -1,109 +1,120 @@ -/** @file - * @brief MAVLink comm protocol enumerations / structures / constants. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include "inttypes.h" - -#define MAVLINK_STX 0xD5 ///< Packet start sign -#define MAVLINK_STX_LEN 1 ///< Length of start sign -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length -//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN - -typedef struct __mavlink_system -{ - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message -{ - union - { - uint16_t ck; ///< Checksum high byte - struct - { - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; - }; - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU -} mavlink_message_t; - -typedef struct __mavlink_header -{ - union { - uint16_t ck; ///< Checksum high byte - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload -} mavlink_header_t; - -typedef enum -{ - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_NB, - MAVLINK_COMM_NB_HIGH = 16 -} mavlink_channel_t; - -typedef enum -{ - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status -{ - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#endif /* MAVLINK_TYPES_H_ */ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system +{ + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode +} mavlink_system_t; + +typedef struct __mavlink_message +{ + union + { + uint16_t ck; ///< Checksum 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 + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header +{ + union + { + uint16_t ck; ///< Checksum 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_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 +} mavlink_channel_t; + +typedef enum +{ + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 4d4c1e52a5..eceea294f9 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -995,51 +995,39 @@ y position 2 / Longitude 2 z position 2 / Altitude 2 - + Set roll, pitch and yaw. System ID Component ID Desired roll angle in radians Desired pitch angle in radians Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 - + Set roll, pitch and yaw. System ID Component ID Desired roll angular speed in rad/s Desired pitch angular speed in rad/s Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 - + Setpoint in roll, pitch, yaw currently active on the system. Timestamp in milliseconds since system boot Desired roll angle in radians Desired pitch angle in radians Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 - + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. Timestamp in milliseconds since system boot Desired roll angular speed in rad/s Desired pitch angular speed in rad/s Desired yaw angular speed in rad/s - - - The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Attitude roll: -128: -100%, 127: +100% - Attitude pitch: -128: -100%, 127: +100% - Attitude yaw: -128: -100%, 127: +100% - Attitude thrust: -128: -100%, 127: +100% - - - The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Position x: -128: -100%, 127: +100% - Position y: -128: -100%, 127: +100% - Position z: -128: -100%, 127: +100% - Position yaw: -128: -100%, 127: +100% + Collective thrust, normalized to 0 .. 1 Outputs of the APM navigation controller. The primary use of this message is to check the response and signs -- GitLab From 4cc06ffcb292e051249f7b504f6a2569623f2fc1 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 13 Aug 2011 09:24:57 +0200 Subject: [PATCH 022/131] Updated MAVLink code generator --- .../mavlinkgen/generator/MAVLinkXMLParser.cc | 233 +++++++--- .../generator/MAVLinkXMLParserV10.cc | 51 +- .../generator/MAVLinkXMLParserV10.h | 1 + src/apps/mavlinkgen/mavlinkgen.pri | 2 + src/apps/mavlinkgen/mavlinkgen.pro | 7 +- src/apps/mavlinkgen/msinttypes/inttypes.h | 305 ++++++++++++ src/apps/mavlinkgen/msinttypes/stdint.h | 247 ++++++++++ src/apps/mavlinkgen/template/checksum.h | 139 ++++++ .../mavlinkgen/template/documentation.dox | 41 ++ .../mavlinkgen/template/mavlink_checksum.h | 183 ++++++++ src/apps/mavlinkgen/template/mavlink_data.h | 21 + .../mavlinkgen/template/mavlink_options.h | 135 ++++++ .../mavlinkgen/template/mavlink_protocol.h | 423 +++++++++++++++++ src/apps/mavlinkgen/template/mavlink_types.h | 120 +++++ src/apps/mavlinkgen/template/protocol.h | 439 ++++++++++++++++++ .../mavlinkgen/ui/XMLCommProtocolWidget.cc | 34 +- .../mavlinkgen/ui/XMLCommProtocolWidget.ui | 45 +- 17 files changed, 2321 insertions(+), 105 deletions(-) create mode 100644 src/apps/mavlinkgen/msinttypes/inttypes.h create mode 100644 src/apps/mavlinkgen/msinttypes/stdint.h create mode 100644 src/apps/mavlinkgen/template/checksum.h create mode 100644 src/apps/mavlinkgen/template/documentation.dox create mode 100644 src/apps/mavlinkgen/template/mavlink_checksum.h create mode 100644 src/apps/mavlinkgen/template/mavlink_data.h create mode 100644 src/apps/mavlinkgen/template/mavlink_options.h create mode 100644 src/apps/mavlinkgen/template/mavlink_protocol.h create mode 100644 src/apps/mavlinkgen/template/mavlink_types.h create mode 100644 src/apps/mavlinkgen/template/protocol.h diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc index 34cc9ec9bc..66769ae158 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc @@ -1,4 +1,24 @@ /*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009 - 2011 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + ======================================================================*/ /** @@ -29,7 +49,8 @@ MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QO { doc = new QDomDocument(); QFile file(document); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { const QString instanceText(QString::fromUtf8(file.readAll())); doc->setContent(instanceText); } @@ -50,7 +71,8 @@ bool MAVLinkXMLParser::generate() bool success = true; // Only generate if output dir is correctly set - if (outputDirName == "") { + if (outputDirName == "") + { emit parseState(tr("ERROR: No output directory given.\nAbort.")); return false; } @@ -96,7 +118,7 @@ bool MAVLinkXMLParser::generate() static int highest_message_id; static int recursion_level; - if (recursion_level == 0) { + if (recursion_level == 0){ highest_message_id = 0; memset(message_lengths, 0, sizeof(message_lengths)); } @@ -113,18 +135,24 @@ bool MAVLinkXMLParser::generate() // Run through root children - while(!n.isNull()) { + while(!n.isNull()) + { // Each child is a message QDomElement e = n.toElement(); // try to convert the node to an element. - if(!e.isNull()) { - if (e.tagName() == "mavlink") { + if(!e.isNull()) + { + if (e.tagName() == "mavlink") + { p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); - if (!e.isNull()) { + if (!e.isNull()) + { // Handle all include tags - if (e.tagName() == "include") { + if (e.tagName() == "include") + { QString incFileName = e.text(); // Load file //QDomDocument includeDoc = QDomDocument(); @@ -134,14 +162,16 @@ bool MAVLinkXMLParser::generate() QFileInfo fInfo(incFileName); QString incFilePath; - if (fInfo.isRelative()) { + if (fInfo.isRelative()) + { QFileInfo rInfo(this->fileName); incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName; pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); } QFile file(incFilePath); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { emit parseState(QString("Included messages from file: %1").arg(incFileName)); // NEW MODE: CREATE INDIVIDUAL FOLDERS // Create new output directory, parse included XML and generate C-code @@ -175,7 +205,9 @@ bool MAVLinkXMLParser::generate() // } emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); - } else { + } + else + { // Include file could not be opened emit parseState(QString("ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.").arg(fileName)); return false; @@ -183,33 +215,40 @@ bool MAVLinkXMLParser::generate() } // Handle all enum tags - else if (e.tagName() == "version") { + else if (e.tagName() == "version") + { //QString fieldType = e.attribute("type", ""); //QString fieldName = e.attribute("name", ""); QString fieldText = e.text(); // Check if version has been previously set - if (mavlinkVersion != 0) { + if (mavlinkVersion != 0) + { emit parseState(QString("ERROR: Protocol version tag set twice, please use it only once. First version was %1, second version is %2.\nAbort.").arg(mavlinkVersion).arg(fieldText)); return false; } bool ok; int version = fieldText.toInt(&ok); - if (ok && (version > 0) && (version < 256)) { + if (ok && (version > 0) && (version < 256)) + { // Set MAVLink version mavlinkVersion = version; - } else { + } + else + { emit parseState(QString("ERROR: Reading version string failed: %1, string is not an integer number between 1 and 255.\nAbort.").arg(fieldText)); return false; } } // Handle all enum tags - else if (e.tagName() == "enums") { + else if (e.tagName() == "enums") + { // One down into the enums list p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); QString currEnum; @@ -217,18 +256,25 @@ bool MAVLinkXMLParser::generate() // Comment QString comment; - if(!e.isNull() && e.tagName() == "enum") { + if(!e.isNull() && e.tagName() == "enum") + { // Get enum name QString enumName = e.attribute("name", "").toLower(); - if (enumName.size() == 0) { + if (enumName.size() == 0) + { emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); return false; - } else { + } + else + { // Sanity check: Accept only enum names not used previously - if (usedEnumNames->contains(enumName)) { + if (usedEnumNames->contains(enumName)) + { emit parseState(tr("ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(enumName, QString::number(e.lineNumber()), fileName)); return false; - } else { + } + else + { usedEnumNames->insert(enumName, QString::number(e.lineNumber())); } @@ -240,22 +286,28 @@ bool MAVLinkXMLParser::generate() // Get the enum fields QDomNode f = e.firstChild(); - while (!f.isNull()) { + while (!f.isNull()) + { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "entry") { + if (!e2.isNull() && e2.tagName() == "entry") + { QString fieldValue = e2.attribute("value", ""); // If value was given, use it, if not, use the enum iterator // value. The iterator value gets reset by manual values QString fieldName = e2.attribute("name", ""); - if (fieldValue.length() == 0) { + if (fieldValue.length() == 0) + { fieldValue = QString::number(nextEnumValue); nextEnumValue++; - } else { + } + else + { bool ok; nextEnumValue = fieldValue.toInt(&ok) + 1; - if (!ok) { + if (!ok) + { emit parseState(tr("ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.").arg(fieldName, fieldValue)); return false; } @@ -267,7 +319,8 @@ bool MAVLinkXMLParser::generate() { QString sep(" | "); QDomNode pp = e2.firstChild(); - while (!pp.isNull()) { + while (!pp.isNull()) + { QDomElement pp2 = pp.toElement(); if (pp2.isText() || pp2.isCDATASection()) { @@ -306,37 +359,49 @@ bool MAVLinkXMLParser::generate() } // Handle all message tags - else if (e.tagName() == "messages") { + else if (e.tagName() == "messages") + { p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); - if(!e.isNull()) { + if(!e.isNull()) + { //if (e.isNull()) continue; // Get message name QString messageName = e.attribute("name", "").toLower(); - if (messageName.size() == 0) { + if (messageName.size() == 0) + { emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); return false; - } else { + } + else + { // Get message id bool ok; int messageId = e.attribute("id", "-1").toInt(&ok, 10); emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); // Sanity check: Accept only message IDs not used previously - if (usedMessageIDs->contains(messageId)) { + if (usedMessageIDs->contains(messageId)) + { emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); return false; - } else { + } + else + { usedMessageIDs->append(messageId); } // Sanity check: Accept only message names not used previously - if (usedMessageNames->contains(messageName)) { + if (usedMessageNames->contains(messageName)) + { emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); return false; - } else { + } + else + { usedMessageNames->insert(messageName, QString::number(e.lineNumber())); } @@ -375,9 +440,11 @@ bool MAVLinkXMLParser::generate() // Get the message fields QDomNode f = e.firstChild(); - while (!f.isNull()) { + while (!f.isNull()) + { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "field") { + if (!e2.isNull() && e2.tagName() == "field") + { QString fieldType = e2.attribute("type", ""); QString fieldName = e2.attribute("name", ""); QString fieldText = e2.text(); @@ -386,7 +453,8 @@ bool MAVLinkXMLParser::generate() QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); // Send arguments do not work for the version field - if (!fieldType.contains("uint8_t_mavlink_version")) { + if (!fieldType.contains("uint8_t_mavlink_version")) + { // Send arguments are the same for integral types and arrays sendArguments += ", " + fieldName; commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); @@ -394,7 +462,8 @@ bool MAVLinkXMLParser::generate() // MAVLink version field // this is a special field always containing the version define - if (fieldType.contains("uint8_t_mavlink_version")) { + if (fieldType.contains("uint8_t_mavlink_version")) + { // Add field to C structure cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); // Add pack line to message_xx_pack function @@ -404,7 +473,8 @@ bool MAVLinkXMLParser::generate() } // Array handling is different from simple types - else if (fieldType.startsWith("array")) { + else if (fieldType.startsWith("array")) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("int8_t*") + " " + fieldName; @@ -417,7 +487,9 @@ bool MAVLinkXMLParser::generate() // Add decode function for this type decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("char*") + " " + fieldName; @@ -432,7 +504,8 @@ bool MAVLinkXMLParser::generate() arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); } // Expand array handling to all valid mavlink data types - else if(fieldType.contains('[') && fieldType.contains(']')) { + else if(fieldType.contains('[') && fieldType.contains(']')) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + arrayType + "* " + fieldName; @@ -452,7 +525,8 @@ bool MAVLinkXMLParser::generate() // decodeLines += ""; prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); - } else + } + else // Handle simple types like integers and floats { packParameters += ", " + fieldType + " " + fieldName; @@ -470,7 +544,8 @@ bool MAVLinkXMLParser::generate() // message length calculation unsigned element_multiplier = 1; unsigned element_length = 0; - const struct { + const struct + { const char *prefix; unsigned length; } length_map[] = { @@ -487,10 +562,12 @@ bool MAVLinkXMLParser::generate() { "float", 4 }, { "double", 8 }, }; - if (fieldType.contains("[")) { + if (fieldType.contains("[")) + { element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); } - for (unsigned i=0; ipayload%2)[0];").arg("uint8_t", prepends); - } else if (fieldType == "uint8_t" || fieldType == "int8_t") { + } + else if (fieldType == "uint8_t" || fieldType == "int8_t") + { unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); - } else if (fieldType == "uint16_t" || fieldType == "int16_t") { + } + else if (fieldType == "uint16_t" || fieldType == "int16_t") + { unpackingCode = QString("\tgeneric_16bit r;\n\tr.b[1] = (msg->payload%1)[0];\n\tr.b[0] = (msg->payload%1)[1];\n\treturn (%2)r.s;").arg(prepends).arg(fieldType); - } else if (fieldType == "uint32_t" || fieldType == "int32_t") { + } + else if (fieldType == "uint32_t" || fieldType == "int32_t") + { unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.i;").arg(prepends).arg(fieldType); - } else if (fieldType == "float") { + } + else if (fieldType == "float") + { unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.f;").arg(prepends).arg(fieldType); - } else if (fieldType == "uint64_t" || fieldType == "int64_t") { + } + else if (fieldType == "uint64_t" || fieldType == "int64_t") + { unpackingCode = QString("\tgeneric_64bit r;\n\tr.b[7] = (msg->payload%1)[0];\n\tr.b[6] = (msg->payload%1)[1];\n\tr.b[5] = (msg->payload%1)[2];\n\tr.b[4] = (msg->payload%1)[3];\n\tr.b[3] = (msg->payload%1)[4];\n\tr.b[2] = (msg->payload%1)[5];\n\tr.b[1] = (msg->payload%1)[6];\n\tr.b[0] = (msg->payload%1)[7];\n\treturn (%2)r.ll;").arg(prepends).arg(fieldType); - } else if (fieldType.startsWith("array")) { + } + else if (fieldType.startsWith("array")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); } // Generate the message decoding function - if (fieldType.contains("uint8_t_mavlink_version")) { + if (fieldType.contains("uint8_t_mavlink_version")) + { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); decodeLines += ""; prepends += "+sizeof(uint8_t)"; } // Array handling is different from simple types - else if (fieldType.startsWith("array")) { + else if (fieldType.startsWith("array")) + { unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } else if(fieldType.contains('[') && fieldType.contains(']')) { + } + else if(fieldType.contains('[') && fieldType.contains(']')) + { // prevent this case from being caught in the following else - } else { + } + else + { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); decodeLines += ""; prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; @@ -553,7 +653,8 @@ bool MAVLinkXMLParser::generate() f = f.nextSibling(); } - if (messageId > highest_message_id) { + if (messageId > highest_message_id) + { highest_message_id = messageId; } message_lengths[messageId] = message_length; @@ -600,7 +701,8 @@ bool MAVLinkXMLParser::generate() mainHeader += "// MESSAGE DEFINITIONS\n\n"; // Create directory if it doesn't exist, report result in success if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName); - for (int i = 0; i < cFiles.size(); i++) { + for (int i = 0; i < cFiles.size(); i++) + { QFile rawFile(dir.filePath(cFiles.at(i).first)); bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text); success = success && ok; @@ -612,7 +714,8 @@ bool MAVLinkXMLParser::generate() mainHeader += "\n\n// MESSAGE LENGTHS\n\n"; mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n"; mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { "; - for (int i=0; i #include #include + #include "MAVLinkXMLParserV10.h" #include @@ -129,8 +130,9 @@ unsigned itemLength( const QString &s1 ) do { if (Ss1.startsWith(length_map[i1].prefix)) + { el1 = length_map[i1].length; - else ; + } i1++; } while ( (el1 == 0) && (i1 < i2)); return el1; @@ -210,7 +212,7 @@ bool MAVLinkXMLParserV10::generate() QString mainHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://qgroundcontrol.org/mavlink/\n *\t Generated on %1\n */\n#ifndef " + pureFileName.toUpper() + "_H\n#define " + pureFileName.toUpper() + "_H\n\n").arg(date); // The main header includes all messages // Mark all code as C code mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n"; - mainHeader += "\n#include \"../protocol.h\"\n"; + mainHeader += "\n#include \"../mavlink_protocol.h\"\n"; mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n"; QString enums; @@ -456,7 +458,7 @@ bool MAVLinkXMLParserV10::generate() // Get message id bool ok; int messageId = e.attribute("id", "-1").toInt(&ok, 10); - emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); +// emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); // Sanity check: Accept only message IDs not used previously if (usedMessageIDs->contains(messageId)) @@ -491,7 +493,7 @@ bool MAVLinkXMLParserV10::generate() QString commentEncodeContainer("/**\n * @brief Encode a %1 struct into a message\n *\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n * @param %1 C-struct to read the message contents from\n */\n"); QString commentDecodeContainer("/**\n * @brief Decode a %1 message into a struct\n *\n * @param msg The message to decode\n * @param %1 C-struct to decode the message contents into\n */\n"); QString commentEntry(" * @param %1 %2\n"); - QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2\n#define MAVLINK_MSG_ID_%1_LEN %3\n#define MAVLINK_MSG_%2_LEN %3"); + QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2\n#define MAVLINK_MSG_ID_%1_LEN %3\n#define MAVLINK_MSG_%2_LEN %3\n#define MAVLINK_MSG_ID_%1_KEY 0x%4\n#define MAVLINK_MSG_%2_KEY 0x%4"); QString arrayDefines; QString cStructName = QString("mavlink_%1_t").arg(messageName); QString cStruct("typedef struct __%1 \n{\n%2\n} %1;"); @@ -552,11 +554,11 @@ bool MAVLinkXMLParserV10::generate() if (fieldType.contains("uint8_t_mavlink_version")) { // Add field to C structure - cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); + cStructLines += QString("\t%1 %2;\t///< %3\n").arg("uint8_t", fieldName, fieldText); calculatedLength += 1; // Add pack line to message_xx_pack function // packLines += QString("\ti += put_uint8_t_by_index(%1, i, msg->payload); // %2\n").arg(mavlinkVersion).arg(fieldText); - packLines += QString("\n\tp->%2 = MAVLINK_VERSION; // %1:%3").arg(fieldType, fieldName, e2.text()); + packLines += QString("\n\tp->%2 = MAVLINK_VERSION;\t// %1:%3").arg(fieldType, fieldName, e2.text()); // Add decode function for this type decodeLines += QString("\n\t%1->%2 = mavlink_msg_%1_get_%2(msg);").arg(messageName, fieldName); @@ -594,10 +596,10 @@ bool MAVLinkXMLParserV10::generate() packArguments += ", " + messageName + "->" + fieldName; // Add field to C structure - cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); + cStructLines += QString("\t%1 %2[%3];\t///< %4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); // Add pack line to message_xx_pack function // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); - packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); + packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2));\t// %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); // Add decode function for this type decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); @@ -663,10 +665,10 @@ bool MAVLinkXMLParserV10::generate() packArguments += ", " + messageName + "->" + fieldName; // Add field to C structure - cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + cStructLines += QString("\t%1 %2[%3];\t///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); // Add pack line to message_xx_pack function // packLines += QString("\ti += put_array_by_index((const int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); - packLines += QString("\tmemcpy(p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + packLines += QString("\tmemcpy(p->%2, %2, sizeof(p->%2));\t// %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); // Add decode function for this type decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); @@ -707,10 +709,10 @@ bool MAVLinkXMLParserV10::generate() packArguments += ", " + messageName + "->" + fieldName; // Add field to C structure - cStructLines += QString("\t%1 %2; ///< %3\n").arg(fieldType, fieldName, fieldText); + cStructLines += QString("\t%1 %2;\t///< %3\n").arg(fieldType, fieldName, fieldText); // Add pack line to message_xx_pack function // packLines += QString("\ti += put_%1_by_index(%2, i, msg->payload); // %3\n").arg(fieldType, fieldName, e2.text()); - packLines += QString("\tp->%2 = %2; // %1:%3\n").arg(fieldType, fieldName, e2.text()); + packLines += QString("\tp->%2 = %2;\t// %1:%3\n").arg(fieldType, fieldName, e2.text()); // Add decode function for this type // decodeLines += QString("\t%1->%2 = mavlink_msg_%1_get_%2(msg);\n").arg(messageName, fieldName); decodeLines += QString("\n\t%1 = p->%1;").arg(fieldName); @@ -882,7 +884,7 @@ bool MAVLinkXMLParserV10::generate() if (fieldList.size() > 1) { qStableSort(fieldList.begin(), fieldList.end(), structSort); - } else ; + } // struct now sorted, do crc calc for each field QString fieldCRCstring; @@ -908,9 +910,7 @@ bool MAVLinkXMLParserV10::generate() // create structure cStructLines = fieldList.join("\n") + "\n"; - - - // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); + // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); cStruct = cStruct.arg(cStructName, cStructLines ); lcmStructDefs.append("\n").append(cStruct).append("\n"); pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); @@ -925,12 +925,13 @@ bool MAVLinkXMLParserV10::generate() // compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); // compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; // QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); - QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); + QString compact2Send0( "\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n" ); + QString compact2Send1("static inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\n\tMAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_%4_LEN )\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); QString compact2Send2("\thdr.sysid = mavlink_system.sysid;\n\thdr.compid = mavlink_system.compid;\n\thdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );\n"); - QString compact2Send3("\n\tcrc_init(&checksum);\n\tchecksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);\n\tchecksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );\n\thdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\thdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);\n}\n\n#endif"); - compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); + QString compact2Send3("\n\tcrc_init(&hdr.ck);\n\tcrc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN);\n\tcrc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len );\n\tcrc_accumulate( 0x%1, &hdr.ck); /// include key in X25 checksum\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES);\n\tMAVLINK_BUFFER_CHECK_END\n}\n\n#endif"); + QString compact2Send = compact2Send0 + commentSendContainer.arg(messageName.toLower(), commentLines) + compact2Send1.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines.replace(QString("p->"),QString("payload.")) ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); // QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; - QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; + QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); emit parseState(tr("Compiled message %1 \t(#%3) \tend near line %2, length %4, crc key 0x%5(%6)").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId), QString::number(message_lengths[messageId]), stringCRC.toUpper(), QString::number(message_key[messageId]))); @@ -985,6 +986,16 @@ bool MAVLinkXMLParserV10::generate() mainHeader += QString::number(message_key[i]); if (i < highest_message_id-1) mainHeader += ", "; } + mainHeader += " }"; + + // Message lengths + mainHeader += "\n\n// MESSAGE LENGTHS\n\n"; + mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n"; + mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { "; + for (int i=0; i #include #include + #include /** diff --git a/src/apps/mavlinkgen/mavlinkgen.pri b/src/apps/mavlinkgen/mavlinkgen.pri index 8270518b47..a5ef920b55 100644 --- a/src/apps/mavlinkgen/mavlinkgen.pri +++ b/src/apps/mavlinkgen/mavlinkgen.pri @@ -29,6 +29,7 @@ FORMS += ui/XMLCommProtocolWidget.ui HEADERS += \ ui/XMLCommProtocolWidget.h \ generator/MAVLinkXMLParser.h \ + generator/MAVLinkXMLParserV10.h \ ui/DomItem.h \ ui/DomModel.h \ ui/QGCMAVLinkTextEdit.h @@ -37,6 +38,7 @@ SOURCES += \ ui/DomItem.cc \ ui/DomModel.cc \ generator/MAVLinkXMLParser.cc \ + generator/MAVLinkXMLParserV10.cc \ ui/QGCMAVLinkTextEdit.cc RESOURCES += mavlinkgen.qrc diff --git a/src/apps/mavlinkgen/mavlinkgen.pro b/src/apps/mavlinkgen/mavlinkgen.pro index 48e3bba526..2786956043 100644 --- a/src/apps/mavlinkgen/mavlinkgen.pro +++ b/src/apps/mavlinkgen/mavlinkgen.pro @@ -13,5 +13,10 @@ include(mavlinkgen.pri) # Standalone files HEADERS += MAVLinkGen.h +win32-msvc2008|win32-msvc2010 { +HEADERS += msinttypes/inttypes.h \ + msinttypes/stdint.h +INCLUDEPATH += msinttypes +} SOURCES += main.cc \ - MAVLinkGen.cc \ No newline at end of file + MAVLinkGen.cc diff --git a/src/apps/mavlinkgen/msinttypes/inttypes.h b/src/apps/mavlinkgen/msinttypes/inttypes.h new file mode 100644 index 0000000000..4b3828a216 --- /dev/null +++ b/src/apps/mavlinkgen/msinttypes/inttypes.h @@ -0,0 +1,305 @@ +// ISO C9x compliant inttypes.h for Microsoft Visual Studio +// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 +// +// Copyright (c) 2006 Alexander Chemeris +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// 3. The name of the author may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED +// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO +// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef _MSC_VER // [ +#error "Use this header only with Microsoft Visual C++ compilers!" +#endif // _MSC_VER ] + +#ifndef _MSC_INTTYPES_H_ // [ +#define _MSC_INTTYPES_H_ + +#if _MSC_VER > 1000 +#pragma once +#endif + +#include "stdint.h" + +// 7.8 Format conversion of integer types + +typedef struct { + intmax_t quot; + intmax_t rem; +} imaxdiv_t; + +// 7.8.1 Macros for format specifiers + +#if !defined(__cplusplus) || defined(__STDC_FORMAT_MACROS) // [ See footnote 185 at page 198 + +// The fprintf macros for signed integers are: +#define PRId8 "d" +#define PRIi8 "i" +#define PRIdLEAST8 "d" +#define PRIiLEAST8 "i" +#define PRIdFAST8 "d" +#define PRIiFAST8 "i" + +#define PRId16 "hd" +#define PRIi16 "hi" +#define PRIdLEAST16 "hd" +#define PRIiLEAST16 "hi" +#define PRIdFAST16 "hd" +#define PRIiFAST16 "hi" + +#define PRId32 "I32d" +#define PRIi32 "I32i" +#define PRIdLEAST32 "I32d" +#define PRIiLEAST32 "I32i" +#define PRIdFAST32 "I32d" +#define PRIiFAST32 "I32i" + +#define PRId64 "I64d" +#define PRIi64 "I64i" +#define PRIdLEAST64 "I64d" +#define PRIiLEAST64 "I64i" +#define PRIdFAST64 "I64d" +#define PRIiFAST64 "I64i" + +#define PRIdMAX "I64d" +#define PRIiMAX "I64i" + +#define PRIdPTR "Id" +#define PRIiPTR "Ii" + +// The fprintf macros for unsigned integers are: +#define PRIo8 "o" +#define PRIu8 "u" +#define PRIx8 "x" +#define PRIX8 "X" +#define PRIoLEAST8 "o" +#define PRIuLEAST8 "u" +#define PRIxLEAST8 "x" +#define PRIXLEAST8 "X" +#define PRIoFAST8 "o" +#define PRIuFAST8 "u" +#define PRIxFAST8 "x" +#define PRIXFAST8 "X" + +#define PRIo16 "ho" +#define PRIu16 "hu" +#define PRIx16 "hx" +#define PRIX16 "hX" +#define PRIoLEAST16 "ho" +#define PRIuLEAST16 "hu" +#define PRIxLEAST16 "hx" +#define PRIXLEAST16 "hX" +#define PRIoFAST16 "ho" +#define PRIuFAST16 "hu" +#define PRIxFAST16 "hx" +#define PRIXFAST16 "hX" + +#define PRIo32 "I32o" +#define PRIu32 "I32u" +#define PRIx32 "I32x" +#define PRIX32 "I32X" +#define PRIoLEAST32 "I32o" +#define PRIuLEAST32 "I32u" +#define PRIxLEAST32 "I32x" +#define PRIXLEAST32 "I32X" +#define PRIoFAST32 "I32o" +#define PRIuFAST32 "I32u" +#define PRIxFAST32 "I32x" +#define PRIXFAST32 "I32X" + +#define PRIo64 "I64o" +#define PRIu64 "I64u" +#define PRIx64 "I64x" +#define PRIX64 "I64X" +#define PRIoLEAST64 "I64o" +#define PRIuLEAST64 "I64u" +#define PRIxLEAST64 "I64x" +#define PRIXLEAST64 "I64X" +#define PRIoFAST64 "I64o" +#define PRIuFAST64 "I64u" +#define PRIxFAST64 "I64x" +#define PRIXFAST64 "I64X" + +#define PRIoMAX "I64o" +#define PRIuMAX "I64u" +#define PRIxMAX "I64x" +#define PRIXMAX "I64X" + +#define PRIoPTR "Io" +#define PRIuPTR "Iu" +#define PRIxPTR "Ix" +#define PRIXPTR "IX" + +// The fscanf macros for signed integers are: +#define SCNd8 "d" +#define SCNi8 "i" +#define SCNdLEAST8 "d" +#define SCNiLEAST8 "i" +#define SCNdFAST8 "d" +#define SCNiFAST8 "i" + +#define SCNd16 "hd" +#define SCNi16 "hi" +#define SCNdLEAST16 "hd" +#define SCNiLEAST16 "hi" +#define SCNdFAST16 "hd" +#define SCNiFAST16 "hi" + +#define SCNd32 "ld" +#define SCNi32 "li" +#define SCNdLEAST32 "ld" +#define SCNiLEAST32 "li" +#define SCNdFAST32 "ld" +#define SCNiFAST32 "li" + +#define SCNd64 "I64d" +#define SCNi64 "I64i" +#define SCNdLEAST64 "I64d" +#define SCNiLEAST64 "I64i" +#define SCNdFAST64 "I64d" +#define SCNiFAST64 "I64i" + +#define SCNdMAX "I64d" +#define SCNiMAX "I64i" + +#ifdef _WIN64 // [ +# define SCNdPTR "I64d" +# define SCNiPTR "I64i" +#else // _WIN64 ][ +# define SCNdPTR "ld" +# define SCNiPTR "li" +#endif // _WIN64 ] + +// The fscanf macros for unsigned integers are: +#define SCNo8 "o" +#define SCNu8 "u" +#define SCNx8 "x" +#define SCNX8 "X" +#define SCNoLEAST8 "o" +#define SCNuLEAST8 "u" +#define SCNxLEAST8 "x" +#define SCNXLEAST8 "X" +#define SCNoFAST8 "o" +#define SCNuFAST8 "u" +#define SCNxFAST8 "x" +#define SCNXFAST8 "X" + +#define SCNo16 "ho" +#define SCNu16 "hu" +#define SCNx16 "hx" +#define SCNX16 "hX" +#define SCNoLEAST16 "ho" +#define SCNuLEAST16 "hu" +#define SCNxLEAST16 "hx" +#define SCNXLEAST16 "hX" +#define SCNoFAST16 "ho" +#define SCNuFAST16 "hu" +#define SCNxFAST16 "hx" +#define SCNXFAST16 "hX" + +#define SCNo32 "lo" +#define SCNu32 "lu" +#define SCNx32 "lx" +#define SCNX32 "lX" +#define SCNoLEAST32 "lo" +#define SCNuLEAST32 "lu" +#define SCNxLEAST32 "lx" +#define SCNXLEAST32 "lX" +#define SCNoFAST32 "lo" +#define SCNuFAST32 "lu" +#define SCNxFAST32 "lx" +#define SCNXFAST32 "lX" + +#define SCNo64 "I64o" +#define SCNu64 "I64u" +#define SCNx64 "I64x" +#define SCNX64 "I64X" +#define SCNoLEAST64 "I64o" +#define SCNuLEAST64 "I64u" +#define SCNxLEAST64 "I64x" +#define SCNXLEAST64 "I64X" +#define SCNoFAST64 "I64o" +#define SCNuFAST64 "I64u" +#define SCNxFAST64 "I64x" +#define SCNXFAST64 "I64X" + +#define SCNoMAX "I64o" +#define SCNuMAX "I64u" +#define SCNxMAX "I64x" +#define SCNXMAX "I64X" + +#ifdef _WIN64 // [ +# define SCNoPTR "I64o" +# define SCNuPTR "I64u" +# define SCNxPTR "I64x" +# define SCNXPTR "I64X" +#else // _WIN64 ][ +# define SCNoPTR "lo" +# define SCNuPTR "lu" +# define SCNxPTR "lx" +# define SCNXPTR "lX" +#endif // _WIN64 ] + +#endif // __STDC_FORMAT_MACROS ] + +// 7.8.2 Functions for greatest-width integer types + +// 7.8.2.1 The imaxabs function +#define imaxabs _abs64 + +// 7.8.2.2 The imaxdiv function + +// This is modified version of div() function from Microsoft's div.c found +// in %MSVC.NET%\crt\src\div.c +#ifdef STATIC_IMAXDIV // [ +static +#else // STATIC_IMAXDIV ][ +_inline +#endif // STATIC_IMAXDIV ] +imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) +{ + imaxdiv_t result; + + result.quot = numer / denom; + result.rem = numer % denom; + + if (numer < 0 && result.rem > 0) { + // did division wrong; must fix up + ++result.quot; + result.rem -= denom; + } + + return result; +} + +// 7.8.2.3 The strtoimax and strtoumax functions +#define strtoimax _strtoi64 +#define strtoumax _strtoui64 + +// 7.8.2.4 The wcstoimax and wcstoumax functions +#define wcstoimax _wcstoi64 +#define wcstoumax _wcstoui64 + + +#endif // _MSC_INTTYPES_H_ ] diff --git a/src/apps/mavlinkgen/msinttypes/stdint.h b/src/apps/mavlinkgen/msinttypes/stdint.h new file mode 100644 index 0000000000..d02608a597 --- /dev/null +++ b/src/apps/mavlinkgen/msinttypes/stdint.h @@ -0,0 +1,247 @@ +// ISO C9x compliant stdint.h for Microsoft Visual Studio +// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 +// +// Copyright (c) 2006-2008 Alexander Chemeris +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// 3. The name of the author may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED +// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO +// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef _MSC_VER // [ +#error "Use this header only with Microsoft Visual C++ compilers!" +#endif // _MSC_VER ] + +#ifndef _MSC_STDINT_H_ // [ +#define _MSC_STDINT_H_ + +#if _MSC_VER > 1000 +#pragma once +#endif + +#include + +// For Visual Studio 6 in C++ mode and for many Visual Studio versions when +// compiling for ARM we should wrap include with 'extern "C++" {}' +// or compiler give many errors like this: +// error C2733: second C linkage of overloaded function 'wmemchr' not allowed +#ifdef __cplusplus +extern "C" { +#endif +# include +#ifdef __cplusplus +} +#endif + +// Define _W64 macros to mark types changing their size, like intptr_t. +#ifndef _W64 +# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300 +# define _W64 __w64 +# else +# define _W64 +# endif +#endif + + +// 7.18.1 Integer types + +// 7.18.1.1 Exact-width integer types + +// Visual Studio 6 and Embedded Visual C++ 4 doesn't +// realize that, e.g. char has the same size as __int8 +// so we give up on __intX for them. +#if (_MSC_VER < 1300) + typedef signed char int8_t; + typedef signed short int16_t; + typedef signed int int32_t; + typedef unsigned char uint8_t; + typedef unsigned short uint16_t; + typedef unsigned int uint32_t; +#else + typedef signed __int8 int8_t; + typedef signed __int16 int16_t; + typedef signed __int32 int32_t; + typedef unsigned __int8 uint8_t; + typedef unsigned __int16 uint16_t; + typedef unsigned __int32 uint32_t; +#endif +typedef signed __int64 int64_t; +typedef unsigned __int64 uint64_t; + + +// 7.18.1.2 Minimum-width integer types +typedef int8_t int_least8_t; +typedef int16_t int_least16_t; +typedef int32_t int_least32_t; +typedef int64_t int_least64_t; +typedef uint8_t uint_least8_t; +typedef uint16_t uint_least16_t; +typedef uint32_t uint_least32_t; +typedef uint64_t uint_least64_t; + +// 7.18.1.3 Fastest minimum-width integer types +typedef int8_t int_fast8_t; +typedef int16_t int_fast16_t; +typedef int32_t int_fast32_t; +typedef int64_t int_fast64_t; +typedef uint8_t uint_fast8_t; +typedef uint16_t uint_fast16_t; +typedef uint32_t uint_fast32_t; +typedef uint64_t uint_fast64_t; + +// 7.18.1.4 Integer types capable of holding object pointers +#ifdef _WIN64 // [ + typedef signed __int64 intptr_t; + typedef unsigned __int64 uintptr_t; +#else // _WIN64 ][ + typedef _W64 signed int intptr_t; + typedef _W64 unsigned int uintptr_t; +#endif // _WIN64 ] + +// 7.18.1.5 Greatest-width integer types +typedef int64_t intmax_t; +typedef uint64_t uintmax_t; + + +// 7.18.2 Limits of specified-width integer types + +#if !defined(__cplusplus) || defined(__STDC_LIMIT_MACROS) // [ See footnote 220 at page 257 and footnote 221 at page 259 + +// 7.18.2.1 Limits of exact-width integer types +#define INT8_MIN ((int8_t)_I8_MIN) +#define INT8_MAX _I8_MAX +#define INT16_MIN ((int16_t)_I16_MIN) +#define INT16_MAX _I16_MAX +#define INT32_MIN ((int32_t)_I32_MIN) +#define INT32_MAX _I32_MAX +#define INT64_MIN ((int64_t)_I64_MIN) +#define INT64_MAX _I64_MAX +#define UINT8_MAX _UI8_MAX +#define UINT16_MAX _UI16_MAX +#define UINT32_MAX _UI32_MAX +#define UINT64_MAX _UI64_MAX + +// 7.18.2.2 Limits of minimum-width integer types +#define INT_LEAST8_MIN INT8_MIN +#define INT_LEAST8_MAX INT8_MAX +#define INT_LEAST16_MIN INT16_MIN +#define INT_LEAST16_MAX INT16_MAX +#define INT_LEAST32_MIN INT32_MIN +#define INT_LEAST32_MAX INT32_MAX +#define INT_LEAST64_MIN INT64_MIN +#define INT_LEAST64_MAX INT64_MAX +#define UINT_LEAST8_MAX UINT8_MAX +#define UINT_LEAST16_MAX UINT16_MAX +#define UINT_LEAST32_MAX UINT32_MAX +#define UINT_LEAST64_MAX UINT64_MAX + +// 7.18.2.3 Limits of fastest minimum-width integer types +#define INT_FAST8_MIN INT8_MIN +#define INT_FAST8_MAX INT8_MAX +#define INT_FAST16_MIN INT16_MIN +#define INT_FAST16_MAX INT16_MAX +#define INT_FAST32_MIN INT32_MIN +#define INT_FAST32_MAX INT32_MAX +#define INT_FAST64_MIN INT64_MIN +#define INT_FAST64_MAX INT64_MAX +#define UINT_FAST8_MAX UINT8_MAX +#define UINT_FAST16_MAX UINT16_MAX +#define UINT_FAST32_MAX UINT32_MAX +#define UINT_FAST64_MAX UINT64_MAX + +// 7.18.2.4 Limits of integer types capable of holding object pointers +#ifdef _WIN64 // [ +# define INTPTR_MIN INT64_MIN +# define INTPTR_MAX INT64_MAX +# define UINTPTR_MAX UINT64_MAX +#else // _WIN64 ][ +# define INTPTR_MIN INT32_MIN +# define INTPTR_MAX INT32_MAX +# define UINTPTR_MAX UINT32_MAX +#endif // _WIN64 ] + +// 7.18.2.5 Limits of greatest-width integer types +#define INTMAX_MIN INT64_MIN +#define INTMAX_MAX INT64_MAX +#define UINTMAX_MAX UINT64_MAX + +// 7.18.3 Limits of other integer types + +#ifdef _WIN64 // [ +# define PTRDIFF_MIN _I64_MIN +# define PTRDIFF_MAX _I64_MAX +#else // _WIN64 ][ +# define PTRDIFF_MIN _I32_MIN +# define PTRDIFF_MAX _I32_MAX +#endif // _WIN64 ] + +#define SIG_ATOMIC_MIN INT_MIN +#define SIG_ATOMIC_MAX INT_MAX + +#ifndef SIZE_MAX // [ +# ifdef _WIN64 // [ +# define SIZE_MAX _UI64_MAX +# else // _WIN64 ][ +# define SIZE_MAX _UI32_MAX +# endif // _WIN64 ] +#endif // SIZE_MAX ] + +// WCHAR_MIN and WCHAR_MAX are also defined in +#ifndef WCHAR_MIN // [ +# define WCHAR_MIN 0 +#endif // WCHAR_MIN ] +#ifndef WCHAR_MAX // [ +# define WCHAR_MAX _UI16_MAX +#endif // WCHAR_MAX ] + +#define WINT_MIN 0 +#define WINT_MAX _UI16_MAX + +#endif // __STDC_LIMIT_MACROS ] + + +// 7.18.4 Limits of other integer types + +#if !defined(__cplusplus) || defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260 + +// 7.18.4.1 Macros for minimum-width integer constants + +#define INT8_C(val) val##i8 +#define INT16_C(val) val##i16 +#define INT32_C(val) val##i32 +#define INT64_C(val) val##i64 + +#define UINT8_C(val) val##ui8 +#define UINT16_C(val) val##ui16 +#define UINT32_C(val) val##ui32 +#define UINT64_C(val) val##ui64 + +// 7.18.4.2 Macros for greatest-width integer constants +#define INTMAX_C INT64_C +#define UINTMAX_C UINT64_C + +#endif // __STDC_CONSTANT_MACROS ] + + +#endif // _MSC_STDINT_H_ ] diff --git a/src/apps/mavlinkgen/template/checksum.h b/src/apps/mavlinkgen/template/checksum.h new file mode 100644 index 0000000000..07bab9102a --- /dev/null +++ b/src/apps/mavlinkgen/template/checksum.h @@ -0,0 +1,139 @@ +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/apps/mavlinkgen/template/documentation.dox b/src/apps/mavlinkgen/template/documentation.dox new file mode 100644 index 0000000000..49de0050e4 --- /dev/null +++ b/src/apps/mavlinkgen/template/documentation.dox @@ -0,0 +1,41 @@ +/** + * @file + * @brief MAVLink communication protocol + * + * @author Lorenz Meier + * + */ + + +/** + * @mainpage MAVLink API Documentation + * + * @section intro_sec Introduction + * + * This API documentation covers the MAVLink + * protocol developed PIXHAWK project. + * In case you have generated this documentation locally, the most recent version (generated on every commit) + * is also publicly available on the internet. + * + * @sa http://pixhawk.ethz.ch/api/qgroundcontrol/ - Groundstation code base + * @sa http://pixhawk.ethz.ch/api/mavlink - (this) MAVLink communication protocol + * @sa http://pixhawk.ethz.ch/api/imu_autopilot/ - Flight board (ARM MCU) code base + * @sa http://pixhawk.ethz.ch/api/ai_vision - Computer Vision / AI API docs + * + * @section further_sec Further Information + * + * How to run our software and a general overview of the software architecture is documented in the project + * wiki pages. + * + * @sa http://pixhawk.ethz.ch/software/mavlink/ - MAVLink main documentation + * + * See the PIXHAWK website for more information. + * + * @section usage_sec Doxygen Usage + * + * You can exclude files from being parsed into this Doxygen documentation + * by adding them to the EXCLUDE list in the file in embedded/cmake/doc/api/doxy.config.in. + * + * + * + **/ diff --git a/src/apps/mavlinkgen/template/mavlink_checksum.h b/src/apps/mavlinkgen/template/mavlink_checksum.h new file mode 100644 index 0000000000..fdcee99d16 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_checksum.h @@ -0,0 +1,183 @@ +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_mem(uint8_t *pBuffer, uint16_t *crcTmp, 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 tmp; + //uint8_t* pTmp; + int i; + +// pTmp=pBuffer; + + for (i = 0; i < length; i++){ + crc_accumulate(*pBuffer++, crcTmp); + } + + return(*crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/apps/mavlinkgen/template/mavlink_data.h b/src/apps/mavlinkgen/template/mavlink_data.h new file mode 100644 index 0000000000..17c88cc2a9 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_data.h @@ -0,0 +1,21 @@ +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +mavlink_system_t mavlink_system; +#endif \ No newline at end of file diff --git a/src/apps/mavlinkgen/template/mavlink_options.h b/src/apps/mavlinkgen/template/mavlink_options.h new file mode 100644 index 0000000000..550a85ae55 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_options.h @@ -0,0 +1,135 @@ +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +//#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STATIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +/** + * + * Convience functions. These are all in one send functions that are very + * easy to use. Just define the symbol MAVLINK_USE_CONVENIENCE_FUNCTIONS. + * These functions also support a buffer check, to ensure there is enough + * space in your comm buffer that the function would not block - it could + * also be used as the basis of a MUTEX. This is implemented in the send + * function as a macro with two arguments, first the comm chan number and + * the message length in the form + * MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LEN ) + * followed by the function code and then + * MAVLINK_BUFFER_CHECK_START + * Note that there are no terminators on these statements to allow for + * code nesting or other constructs. Default value for both is empty. + * A sugested implementation is shown below and the symbols will be defined + * only if they are not allready. + * + * if ( serial_space( chan ) > len ) { // serial_space returns available space + * ..... code that creates message + * } + * + * #define MAVLINK_BUFFER_CHECK_START( c, l ) if ( serial_space( c ) > l ) { + * #define MAVLINK_BUFFER_CHECK_END } + * + */ +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +#ifndef MAVLINK_BUFFER_CHECK_START +#define MAVLINK_BUFFER_CHECK_START( c, l ) ; +#endif +#ifndef MAVLINK_BUFFER_CHECK_END +#define MAVLINK_BUFFER_CHECK_END ; +#endif + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/apps/mavlinkgen/template/mavlink_protocol.h b/src/apps/mavlinkgen/template/mavlink_protocol.h new file mode 100644 index 0000000000..8cf62f115d --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_protocol.h @@ -0,0 +1,423 @@ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +extern mavlink_system_t mavlink_system; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + 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; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + 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; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->ck); +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->ck); +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STATIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + { + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + break; + } else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; +#ifdef MAVLINK_STATIC_BUFFER + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; +#else + if (status->msg_received == 1) + return 1; + else return 0; +#endif +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, uint8_t *mem, uint16_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/src/apps/mavlinkgen/template/mavlink_types.h b/src/apps/mavlinkgen/template/mavlink_types.h new file mode 100644 index 0000000000..77b2e24096 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_types.h @@ -0,0 +1,120 @@ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system +{ + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode +} mavlink_system_t; + +typedef struct __mavlink_message +{ + union + { + uint16_t ck; ///< Checksum 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 + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header +{ + union + { + uint16_t ck; ///< Checksum 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_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 +} mavlink_channel_t; + +typedef enum +{ + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/src/apps/mavlinkgen/template/protocol.h b/src/apps/mavlinkgen/template/protocol.h new file mode 100644 index 0000000000..a45993e692 --- /dev/null +++ b/src/apps/mavlinkgen/template/protocol.h @@ -0,0 +1,439 @@ +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "string.h" +#include "mavlink_types.h" + +#include "checksum.h" + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +#endif + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + union checksum_ ck; + crc_init(&(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + union checksum_ ck; + ck.c[0] = msg->ck_a; + ck.c[1] = msg->ck_b; + crc_accumulate(c, &(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +#elif defined(NB_MAVLINK_COMM) + static mavlink_message_t m_mavlink_message[NB_MAVLINK_COMM]; +#else + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +#endif + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + 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 ; +#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 + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc index 337a019581..37cbd19cff 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc @@ -6,6 +6,7 @@ #include "XMLCommProtocolWidget.h" #include "ui_XMLCommProtocolWidget.h" #include "MAVLinkXMLParser.h" +#include "MAVLinkXMLParserV10.h" #include #include @@ -131,18 +132,37 @@ void XMLCommProtocolWidget::generate() // Syntax check already gives output return; } - - MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); - connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); - bool result = parser->generate(); - if (result) { + + MAVLinkXMLParser* parser = NULL; + MAVLinkXMLParserV10* parserV10 = NULL; + + bool result = false; + + if (m_ui->versionComboBox->currentIndex() == 1) + { + MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + result = parser->generate(); + } + else if (m_ui->versionComboBox->currentIndex() == 0) + { + MAVLinkXMLParserV10* parserV10 = new MAVLinkXMLParserV10(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parserV10, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + result = parserV10->generate(); + } + + if (result) + { QMessageBox msgBox; msgBox.setText(QString("The C code / headers have been generated in folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed())); msgBox.exec(); - } else { + } + else + { QMessageBox::critical(this, tr("C code generation failed, please see the compile log for further information"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok); } - delete parser; + if (parser) delete parser; + if (parserV10) delete parserV10; } void XMLCommProtocolWidget::save() diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui index 089c8afa76..41af4832f7 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui @@ -13,7 +13,7 @@ Form - + 6 @@ -51,12 +51,12 @@ Select input file - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + @@ -97,49 +97,70 @@ Select directory - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + - + Compile Output - + - + No file loaded - + Save file - + Save and generate - + :/images/categories/applications-system.svg:/images/categories/applications-system.svg + + + + Select MAVLink Version + + + + + + + + MAVLink v1.0 (Sept'10+) + + + + + MAVLink v0.9 (-Aug'10) + + + + @@ -150,7 +171,7 @@ - + -- GitLab From 154f730a0a37fe6f35396a2d7cbd6ec9769d62f4 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 13 Aug 2011 09:26:38 +0200 Subject: [PATCH 023/131] Updated MAVlink --- .../include/ardupilotmega/ardupilotmega.h | 11 +- .../mavlink/include/ardupilotmega/mavlink.h | 2 +- thirdParty/mavlink/include/common/common.h | 4 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- thirdParty/mavlink/include/mavlink_protocol.h | 467 ++++--- thirdParty/mavlink/include/minimal/mavlink.h | 2 +- .../include/minimal/mavlink_msg_heartbeat.h | 48 +- thirdParty/mavlink/include/minimal/minimal.h | 9 +- thirdParty/mavlink/include/pixhawk/mavlink.h | 2 +- .../pixhawk/mavlink_msg_attitude_control.h | 96 +- .../include/pixhawk/mavlink_msg_aux_status.h | 72 +- .../pixhawk/mavlink_msg_brief_feature.h | 90 +- .../mavlink_msg_data_transmission_handshake.h | 64 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 40 +- .../pixhawk/mavlink_msg_image_available.h | 210 ++-- .../mavlink_msg_image_trigger_control.h | 32 +- .../pixhawk/mavlink_msg_image_triggered.h | 120 +- .../include/pixhawk/mavlink_msg_marker.h | 80 +- .../pixhawk/mavlink_msg_pattern_detected.h | 56 +- .../pixhawk/mavlink_msg_point_of_interest.h | 90 +- ...mavlink_msg_point_of_interest_connection.h | 112 +- .../mavlink_msg_position_control_offset_set.h | 72 +- .../mavlink_msg_position_control_setpoint.h | 64 +- ...avlink_msg_position_control_setpoint_set.h | 80 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 80 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 72 +- .../mavlink_msg_vicon_position_estimate.h | 80 +- .../mavlink_msg_vision_position_estimate.h | 80 +- .../mavlink_msg_vision_speed_estimate.h | 56 +- .../pixhawk/mavlink_msg_watchdog_command.h | 56 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 40 +- .../mavlink_msg_watchdog_process_info.h | 64 +- .../mavlink_msg_watchdog_process_status.h | 72 +- thirdParty/mavlink/include/pixhawk/pixhawk.h | 11 +- thirdParty/mavlink/include/slugs/mavlink.h | 2 +- .../include/slugs/mavlink_msg_air_data.h | 48 +- .../include/slugs/mavlink_msg_cpu_load.h | 48 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 40 +- .../include/slugs/mavlink_msg_data_log.h | 72 +- .../include/slugs/mavlink_msg_diagnostic.h | 72 +- .../include/slugs/mavlink_msg_gps_date_time.h | 80 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 56 +- .../include/slugs/mavlink_msg_sensor_bias.h | 72 +- .../include/slugs/mavlink_msg_slugs_action.h | 48 +- .../slugs/mavlink_msg_slugs_navigation.h | 96 +- thirdParty/mavlink/include/slugs/slugs.h | 11 +- thirdParty/mavlink/include/ualberta/mavlink.h | 2 +- .../ualberta/mavlink_msg_nav_filter_bias.h | 80 +- .../ualberta/mavlink_msg_radio_calibration.h | 72 +- .../mavlink_msg_ualberta_sys_status.h | 48 +- .../mavlink/include/ualberta/ualberta.h | 11 +- .../mavlink/missionlib/mavlink_parameters.c | 146 +++ .../mavlink/missionlib/mavlink_parameters.h | 42 + thirdParty/mavlink/missionlib/testing/main.c | 1014 +--------------- .../testing/mavlink_missionlib_data.h | 54 + thirdParty/mavlink/missionlib/testing/udp.c | 1073 ----------------- thirdParty/mavlink/missionlib/waypoints.c | 58 +- thirdParty/mavlink/missionlib/waypoints.h | 10 +- 58 files changed, 1960 insertions(+), 3731 deletions(-) create mode 100644 thirdParty/mavlink/missionlib/mavlink_parameters.c create mode 100644 thirdParty/mavlink/missionlib/mavlink_parameters.h create mode 100644 thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h delete mode 100644 thirdParty/mavlink/missionlib/testing/udp.c diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 774e3e48df..2b21073894 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_ARDUPILOTMEGA @@ -38,7 +38,12 @@ extern "C" { // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 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 { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 933149099f..e0e17b063e 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 2364eb3ecd..b14f77c31a 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 12 2011, 20:25 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef COMMON_H #define COMMON_H @@ -300,7 +300,7 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 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 diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 9c3399f455..b7cbe96da4 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 12 2011, 20:25 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h index 596b5ffcf1..8cf62f115d 100644 --- a/thirdParty/mavlink/include/mavlink_protocol.h +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -31,26 +31,26 @@ extern mavlink_system_t mavlink_system; */ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) { - if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) - { - initStatus->ck_a = 0; - initStatus->ck_b = 0; - initStatus->msg_received = 0; - initStatus->buffer_overrun = 0; - initStatus->parse_error = 0; - initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; - initStatus->packet_idx = 0; - initStatus->packet_rx_drop_count = 0; - initStatus->packet_rx_success_count = 0; - initStatus->current_rx_seq = 0; - initStatus->current_tx_seq = 0; - } + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } } static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { - return &m_mavlink_status[chan]; + return &m_mavlink_status[chan]; } /** @@ -76,19 +76,19 @@ static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) */ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) { - // This code part is the same for all messages; - uint8_t key; - 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; - msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); - crc_accumulate( key, &msg->ck ); /// include key in X25 checksum - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; + // This code part is the same for all messages; + uint8_t key; + 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; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; } /** @@ -105,19 +105,19 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t */ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) { - // This code part is the same for all messages; - uint8_t key; - 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; - msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); - crc_accumulate( key, &msg->ck ); /// include key in X25 checksum - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; + // This code part is the same for all messages; + uint8_t key; + 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; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; } /** @@ -125,14 +125,14 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin */ static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) { - *(buffer+0) = MAVLINK_STX; ///< Start transmit - // memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + *(buffer+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; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; } /** @@ -140,22 +140,22 @@ static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink */ static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) { - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; } union checksum_ { - uint16_t s; - uint8_t c[2]; + uint16_t s; + uint8_t c[2]; }; static inline void mavlink_start_checksum(mavlink_message_t* msg) { - crc_init(&msg->ck); + crc_init(&msg->ck); } static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) { - crc_accumulate(c, &msg->ck); + crc_accumulate(c, &msg->ck); } /** @@ -200,172 +200,167 @@ static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mav static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) #endif { - // Initializes only once, values keep unchanged after first initialization - mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); - - mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); #ifdef MAVLINK_CHECK_LENGTH - if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) - { - status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway - break; - } else ; + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + { + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + break; + } else ; #endif - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - rxmsg->payload[status->packet_idx++] = c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - mavlink_update_checksum(rxmsg, - MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: - if (c != rxmsg->ck_a) - { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != rxmsg->ck_b) - {// Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if ( r_message != NULL ) - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - else ; - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; #ifdef MAVLINK_STATIC_BUFFER - if (status->msg_received == 1) - { - if ( r_message != NULL ) - { - return r_message; - } - else - { - return rxmsg; - } - } else return NULL; + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; #else - if (status->msg_received == 1) - return 1; - else return 0; + if (status->msg_received == 1) + return 1; + else return 0; #endif } @@ -385,39 +380,39 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch) } if (chan == MAVLINK_COMM_1) { - uart1_transmit(ch); + uart1_transmit(ch); } } static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) { - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - comm_send_ch(chan, MAVLINK_STX); - comm_send_ch(chan, msg->len); - comm_send_ch(chan, msg->seq); - comm_send_ch(chan, msg->sysid); - comm_send_ch(chan, msg->compid); - comm_send_ch(chan, msg->msgid); - for(uint16_t i = 0; i < msg->len; i++) - { - comm_send_ch(chan, msg->payload[i]); - } - comm_send_ch(chan, msg->ck_a); - comm_send_ch(chan, msg->ck_b); + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); } static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) { - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - for(uint16_t i = 0; i < num; i++) - { - comm_send_ch( chan, mem[i] ); - } + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } } */ static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index c8cd1940e2..3727dd55d3 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 7c686831ee..466f8a783d 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_HEARTBEAT 0 #define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 #define MAVLINK_MSG_0_LEN 3 +#define MAVLINK_MSG_ID_HEARTBEAT_KEY 0x47 +#define MAVLINK_MSG_0_KEY 0x47 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; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - 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; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + uint8_t mavlink_version; ///< MAVLink version } mavlink_heartbeat_t; @@ -27,10 +29,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com 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:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + 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); } @@ -49,10 +51,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ 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:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + 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); } @@ -69,6 +71,8 @@ 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); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -76,20 +80,16 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; mavlink_heartbeat_t payload; - uint16_t checksum; - mavlink_heartbeat_t *p = &payload; - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + 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:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + 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; @@ -99,14 +99,12 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0x47, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index c3d7e2a1bf..455b0735a8 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_MINIMAL @@ -39,6 +39,11 @@ extern "C" { #undef MAVLINK_MESSAGE_KEYS #define MAVLINK_MESSAGE_KEYS { } +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index b14edfc4d0..c6ab3b1c07 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:49 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index fd8503d974..3da14afabf 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_ATTITUDE_CONTROL 85 #define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21 #define MAVLINK_MSG_85_LEN 21 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_KEY 0x7F +#define MAVLINK_MSG_85_KEY 0x7F typedef struct __mavlink_attitude_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_attitude_control_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint 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 + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, 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 + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_attitude_control_t *p = &payload; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index 2fb3314c93..4f470bd84a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_AUX_STATUS 142 #define MAVLINK_MSG_ID_AUX_STATUS_LEN 12 #define MAVLINK_MSG_142_LEN 12 +#define MAVLINK_MSG_ID_AUX_STATUS_KEY 0x7A +#define MAVLINK_MSG_142_KEY 0x7A typedef struct __mavlink_aux_status_t { - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t i2c0_err_count; ///< Number of I2C errors since startup - uint16_t i2c1_err_count; ///< Number of I2C errors since startup - uint16_t spi0_err_count; ///< Number of I2C errors since startup - uint16_t spi1_err_count; ///< Number of I2C errors since startup - uint16_t uart_total_err_count; ///< Number of I2C errors since startup + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t i2c0_err_count; ///< Number of I2C errors since startup + uint16_t i2c1_err_count; ///< Number of I2C errors since startup + uint16_t spi0_err_count; ///< Number of I2C errors since startup + uint16_t spi1_err_count; ///< Number of I2C errors since startup + uint16_t uart_total_err_count; ///< Number of I2C errors since startup } mavlink_aux_status_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t co mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUX_STATUS_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_aux_status_pack_chan(uint8_t system_id, uint8 mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUX_STATUS_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t return mavlink_msg_aux_status_pack(system_id, component_id, msg, aux_status->load, aux_status->i2c0_err_count, aux_status->i2c1_err_count, aux_status->spi0_err_count, aux_status->spi1_err_count, aux_status->uart_total_err_count); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a aux_status message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param spi1_err_count Number of I2C errors since startup * @param uart_total_err_count Number of I2C errors since startup */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { mavlink_header_t hdr; mavlink_aux_status_t payload; - uint16_t checksum; - mavlink_aux_status_t *p = &payload; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_AUX_STATUS_LEN ) + payload.load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + payload.i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + payload.i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + payload.spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + payload.spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + payload.uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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( 0x7A, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index 4e6f6b83bc..cc0e71398b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -3,17 +3,19 @@ #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 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_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 @@ -39,14 +41,14 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t 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 + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } @@ -72,14 +74,14 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui 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 + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } @@ -97,6 +99,8 @@ 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 @@ -110,24 +114,20 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param descriptor Descriptor * @param response Harris operator response at this location */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_brief_feature_t *p = &payload; - - p->x = x; // float:x position in m - p->y = y; // float:y position in m - p->z = z; // float:z position in m - p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true - p->size = size; // uint16_t:Size in pixels - p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor - p->response = response; // float:Harris operator response at this location + + 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; @@ -138,14 +138,12 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 54d2070a98..e329ac1025 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -3,14 +3,16 @@ #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 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; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst 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] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t 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] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } @@ -81,6 +83,8 @@ 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 @@ -91,21 +95,17 @@ 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; - uint16_t checksum; - mavlink_data_transmission_handshake_t *p = &payload; - p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - p->size = size; // uint32_t:total data size in bytes (set on ACK only) - p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) - p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + 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; @@ -116,14 +116,12 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index a5354fdded..4fc69dfb98 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -3,11 +3,13 @@ #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 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_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin 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 + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id 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 + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } @@ -67,6 +69,8 @@ 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 @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_encapsulated_data_t *p = &payload; - p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) - memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + 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; @@ -96,14 +96,12 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index 0b4c9c8eff..2bb279c218 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -3,32 +3,34 @@ #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 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; @@ -68,29 +70,29 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 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 + 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); } @@ -131,29 +133,29 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, 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 + 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); } @@ -171,6 +173,8 @@ 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 @@ -199,39 +203,35 @@ 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; - uint16_t checksum; - mavlink_image_available_t *p = &payload; - - p->cam_id = cam_id; // uint64_t:Camera id - p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) - p->timestamp = timestamp; // uint64_t:Timestamp - p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid - p->img_seq = img_seq; // uint32_t:The image sequence number - p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 - p->width = width; // uint16_t:Image width - p->height = height; // uint16_t:Image height - p->depth = depth; // uint16_t:Image depth - p->channels = channels; // uint8_t:Image channels - p->key = key; // uint32_t:Shared memory area key - p->exposure = exposure; // uint32_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + + 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; @@ -242,14 +242,12 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 99027fea5b..4ab1dad96f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -3,10 +3,12 @@ #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 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; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, 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 + p->enable = enable; // uint8_t:0 to disable, 1 to enable return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste 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 + p->enable = enable; // uint8_t:0 to disable, 1 to enable return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } @@ -61,23 +63,21 @@ 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; - uint16_t checksum; - mavlink_image_trigger_control_t *p = &payload; - p->enable = enable; // uint8_t:0 to disable, 1 to enable + 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; @@ -88,14 +88,12 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 250bee3a1b..6fca6a1cf7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -3,21 +3,23 @@ #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 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; @@ -46,18 +48,18 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 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 + 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); } @@ -87,18 +89,18 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, 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 + 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); } @@ -116,6 +118,8 @@ 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 @@ -133,28 +137,24 @@ 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; - uint16_t checksum; - mavlink_image_triggered_t *p = &payload; - p->timestamp = timestamp; // uint64_t:Timestamp - p->seq = seq; // uint32_t:IMU seq - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + 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; @@ -165,14 +165,12 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index 34ebf63517..a5074c706d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -3,16 +3,18 @@ #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 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon 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 + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c 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 + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_marker_t *p = &payload; - p->id = id; // uint16_t:ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->roll = roll; // float:roll orientation - p->pitch = pitch; // float:pitch orientation - p->yaw = yaw; // float:yaw orientation + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index d5d803bd0c..2ea0fc79c0 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -3,13 +3,15 @@ #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 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_PATTERN_DETECTED_FIELD_FILE_LEN 100 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint 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 + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, 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 + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } @@ -77,6 +79,8 @@ 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 @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param file Pattern file name * @param detected Accepted as true detection, 0 no, 1 yes */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) { mavlink_header_t hdr; mavlink_pattern_detected_t payload; - uint16_t checksum; - mavlink_pattern_detected_t *p = &payload; - p->type = type; // uint8_t:0: Pattern, 1: Letter - p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name - p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + 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; @@ -110,14 +110,12 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 148d8f784e..f3ae2efca4 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -3,17 +3,19 @@ #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 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_POINT_OF_INTEREST_FIELD_NAME_LEN 26 @@ -39,14 +41,14 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin 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 + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -72,14 +74,14 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id 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 + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -97,6 +99,8 @@ 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 @@ -110,24 +114,20 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param z Z Position * @param name POI name */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { mavlink_header_t hdr; mavlink_point_of_interest_t payload; - uint16_t checksum; - mavlink_point_of_interest_t *p = &payload; - - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name + + 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; @@ -138,14 +138,12 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 7accdbc9c3..75f26249f8 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 @@ -3,20 +3,22 @@ #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 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_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 @@ -45,17 +47,17 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys 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 + 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); } @@ -84,17 +86,17 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ 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 + 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); } @@ -112,6 +114,8 @@ 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 @@ -128,27 +132,23 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param zp2 Z2 Position * @param name POI connection name */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { mavlink_header_t hdr; mavlink_point_of_interest_connection_t payload; - uint16_t checksum; - mavlink_point_of_interest_connection_t *p = &payload; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->xp1 = xp1; // float:X1 Position - p->yp1 = yp1; // float:Y1 Position - p->zp1 = zp1; // float:Z1 Position - p->xp2 = xp2; // float:X2 Position - p->yp2 = yp2; // float:Y2 Position - p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name + 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; @@ -159,14 +159,12 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 b5446931f4..3ae1a4107d 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 @@ -3,15 +3,17 @@ #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 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t syst 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack_chan(uint8_t 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_position_control_offset_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position offset - p->y = y; // float:y position offset - p->z = z; // float:z position offset - p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 dd3b92ca8c..a40eea0c9f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -3,14 +3,16 @@ #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 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; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system 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 + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s 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 + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } @@ -81,6 +83,8 @@ 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 @@ -91,21 +95,17 @@ 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; - uint16_t checksum; - mavlink_position_control_setpoint_t *p = &payload; - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + 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; @@ -116,14 +116,12 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 f14a31c467..e22c10bf6d 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 @@ -3,16 +3,18 @@ #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 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t sy 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack_chan(uint8 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_position_control_setpoint_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channe mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 7bafeb4c99..917935346b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -3,16 +3,18 @@ #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 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo 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) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t 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) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_raw_aux_t *p = &payload; - p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) - p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) - p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) - p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) - p->vbat = vbat; // uint16_t:Battery voltage - p->temp = temp; // int16_t:Temperature (degrees celcius) - p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 5b14662365..0cc93ff58c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -3,15 +3,17 @@ #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 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 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 + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, 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 + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_set_cam_shutter_t *p = &payload; - p->cam_no = cam_no; // uint8_t:Camera id - p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual - p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly - p->interval = interval; // uint16_t:Shutter interval, in microseconds - p->exposure = exposure; // uint16_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 862ea8cbef..462e3801fb 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -3,16 +3,18 @@ #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 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i 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 + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys 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 + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_vicon_position_estimate_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 cfd0b15f7c..1bd2ca2506 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -3,16 +3,18 @@ #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 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ 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 + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy 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 + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_vision_position_estimate_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 e3d0755de3..d9bfe7b634 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -3,13 +3,15 @@ #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 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, 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 + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste 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 + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_vision_speed_estimate_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X speed - p->y = y; // float:Global Y speed - p->z = z; // float:Global Z speed + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index dee5bcf0aa..8acb62e2da 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -3,13 +3,15 @@ #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 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint 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 + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_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 + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_watchdog_command_t *p = &payload; - p->target_system_id = target_system_id; // uint8_t:Target system ID - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->command_id = command_id; // uint8_t:Command ID + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index e31a060ead..5e4ac05f53 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -3,11 +3,13 @@ #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 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui 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 + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i 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 + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_watchdog_heartbeat_t *p = &payload; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 e9d7703f9e..36b2480080 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -3,14 +3,16 @@ #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 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_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 @@ -34,11 +36,11 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, 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) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } @@ -61,11 +63,11 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste 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) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } @@ -83,6 +85,8 @@ 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 @@ -93,21 +97,17 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param arguments Process arguments * @param timeout Timeout (seconds) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { mavlink_header_t hdr; mavlink_watchdog_process_info_t payload; - uint16_t checksum; - mavlink_watchdog_process_info_t *p = &payload; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments - p->timeout = timeout; // int32_t:Timeout (seconds) + 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; @@ -118,14 +118,12 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 f6f3dd26d7..7765a92406 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -3,15 +3,17 @@ #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 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i 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 + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys 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 + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_watchdog_process_status_t *p = &payload; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->state = state; // uint8_t:Is running / finished / suspended / crashed - p->muted = muted; // uint8_t:Is muted - p->pid = pid; // int32_t:PID - p->crashes = crashes; // uint16_t:Number of crashes + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index a0bd474b19..a94bde4f28 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:49 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_PIXHAWK @@ -71,7 +71,12 @@ enum DATA_TYPES // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 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 { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 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 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index 04bffe2e31..cbf9b6ae3b 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef MAVLINK_H #define 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 b345be106d..b1858d990a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t comp 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 + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIR_DATA_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t 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 + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIR_DATA_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_air_data_t *p = &payload; - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynam mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index f30e1d8b62..42efc6407b 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t comp 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 + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CPU_LOAD_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t 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 + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CPU_LOAD_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_cpu_load_t *p = &payload; - p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load - p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load - p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sen mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 81b5fa59a2..604412c4c8 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -3,11 +3,13 @@ #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 { - 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; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t 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 + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uin 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 + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } @@ -66,6 +68,8 @@ 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 @@ -73,18 +77,14 @@ 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; - uint16_t checksum; - mavlink_ctrl_srfc_pt_t *p = &payload; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + 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; @@ -95,14 +95,12 @@ static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index 14da8b9a99..2f3a881c49 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -3,15 +3,17 @@ #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 { - 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t comp 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 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_LOG_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t 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 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_LOG_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_data_log_t *p = &payload; - p->fl_1 = fl_1; // float:Log value 1 - p->fl_2 = fl_2; // float:Log value 2 - p->fl_3 = fl_3; // float:Log value 3 - p->fl_4 = fl_4; // float:Log value 4 - p->fl_5 = fl_5; // float:Log value 5 - p->fl_6 = fl_6; // float:Log value 6 + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index e9aa6279b9..40a1ef617e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -3,15 +3,17 @@ #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 { - 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t co 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 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8 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 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_diagnostic_t *p = &payload; - p->diagFl1 = diagFl1; // float:Diagnostic float 1 - p->diagFl2 = diagFl2; // float:Diagnostic float 2 - p->diagFl3 = diagFl3; // float:Diagnostic float 3 - p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 - p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 - p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float dia mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 bab695cfa4..ba616ad3c6 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -3,16 +3,18 @@ #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 { - 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; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t 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 + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, ui 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 + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_gps_date_time_t *p = &payload; - p->year = year; // uint8_t:Year reported by Gps - p->month = month; // uint8_t:Month reported by Gps - p->day = day; // uint8_t:Day reported by Gps - p->hour = hour; // uint8_t:Hour reported by Gps - p->min = min; // uint8_t:Min reported by Gps - p->sec = sec; // uint8_t:Sec reported by Gps - p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 0436ae9498..15c1474428 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -3,13 +3,15 @@ #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 { - 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; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t 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 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uin 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 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } @@ -76,6 +78,8 @@ 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 @@ -85,20 +89,16 @@ 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; - uint16_t checksum; - mavlink_mid_lvl_cmds_t *p = &payload; - p->target = target; // uint8_t:The system setting the commands - p->hCommand = hCommand; // float:Commanded Airspeed - p->uCommand = uCommand; // float:Log value 2 - p->rCommand = rCommand; // float:Log value 3 + 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; @@ -109,14 +109,12 @@ static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 5601a2a344..25d9fb339f 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -3,15 +3,17 @@ #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 { - 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; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t c 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) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint 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) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } @@ -86,6 +88,8 @@ 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 @@ -97,22 +101,18 @@ 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; - uint16_t checksum; - mavlink_sensor_bias_t *p = &payload; - p->axBias = axBias; // float:Accelerometer X bias (m/s) - p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) - p->azBias = azBias; // float:Accelerometer Z bias (m/s) - p->gxBias = gxBias; // float:Gyro X bias (rad/s) - p->gyBias = gyBias; // float:Gyro Y bias (rad/s) - p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + 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; @@ -123,14 +123,12 @@ static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float ax mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index fe3399e2ec..d073928c2a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t 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 + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uin 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 + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_slugs_action_t *p = &payload; - p->target = target; // uint8_t:The system reporting the action - p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - p->actionVal = actionVal; // uint16_t:Value associated with the action + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index dc69df79e8..f3e337cf17 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -3,18 +3,20 @@ #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 { - 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; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint 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 + 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); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, 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 + 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); } @@ -101,6 +103,8 @@ 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 @@ -115,25 +119,21 @@ 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; - uint16_t checksum; - mavlink_slugs_navigation_t *p = &payload; - p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter - p->phi_c = phi_c; // float:Commanded Roll - p->theta_c = theta_c; // float:Commanded Pitch - p->psiDot_c = psiDot_c; // float:Commanded Turn rate - p->ay_body = ay_body; // float:Y component of the body acceleration - p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation - p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation - p->fromWP = fromWP; // uint8_t:Origin WP - p->toWP = toWP; // uint8_t:Destination WP + 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; @@ -144,14 +144,12 @@ static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, flo mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index 990857c729..eae6c7e302 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_SLUGS @@ -48,7 +48,12 @@ extern "C" { // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 0bb9f2f906..591a133261 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index d1b777ee46..6971ea4083 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_NAV_FILTER_BIAS 220 #define MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN 32 #define MAVLINK_MSG_220_LEN 32 +#define MAVLINK_MSG_ID_NAV_FILTER_BIAS_KEY 0xFF +#define MAVLINK_MSG_220_KEY 0xFF typedef struct __mavlink_nav_filter_bias_t { - uint64_t usec; ///< Timestamp (microseconds) - float accel_0; ///< b_f[0] - float accel_1; ///< b_f[1] - float accel_2; ///< b_f[2] - float gyro_0; ///< b_f[0] - float gyro_1; ///< b_f[1] - float gyro_2; ///< b_f[2] + uint64_t usec; ///< Timestamp (microseconds) + float accel_0; ///< b_f[0] + float accel_1; ///< b_f[1] + float accel_2; ///< b_f[2] + float gyro_0; ///< b_f[0] + float gyro_1; ///< b_f[1] + float gyro_2; ///< b_f[2] } mavlink_nav_filter_bias_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8 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] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, 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] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } @@ -91,6 +93,8 @@ 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 @@ -103,23 +107,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; - uint16_t checksum; - mavlink_nav_filter_bias_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds) - p->accel_0 = accel_0; // float:b_f[0] - p->accel_1 = accel_1; // float:b_f[1] - p->accel_2 = accel_2; // float:b_f[2] - p->gyro_0 = gyro_0; // float:b_f[0] - p->gyro_1 = gyro_1; // float:b_f[1] - p->gyro_2 = gyro_2; // float:b_f[2] + 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; @@ -130,14 +130,12 @@ static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 22b139afc6..107c5f8609 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -3,15 +3,17 @@ #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 { - 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_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 @@ -40,12 +42,12 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin 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%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } @@ -69,12 +71,12 @@ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id 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%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } @@ -92,6 +94,8 @@ 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 @@ -103,22 +107,18 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param pitch Pitch curve setpoints (every 25%) * @param throttle Throttle curve setpoints (every 25%) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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; - uint16_t checksum; - mavlink_radio_calibration_t *p = &payload; - memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right - memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up - memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right - memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode - memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) - memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + 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; @@ -129,14 +129,12 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #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 16e2ff6853..a42653e982 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -3,12 +3,14 @@ #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 { - 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; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, u 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 + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_ 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 + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } @@ -71,6 +73,8 @@ 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 @@ -79,19 +83,15 @@ 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; - uint16_t checksum; - mavlink_ualberta_sys_status_t *p = &payload; - p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM - p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + 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; @@ -102,14 +102,12 @@ static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + 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 } #endif diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index c0c03d8279..90cf2cd08f 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_UALBERTA @@ -71,7 +71,12 @@ enum UALBERTA_PILOT_MODE // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 73, 239, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 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, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/missionlib/mavlink_parameters.c b/thirdParty/mavlink/missionlib/mavlink_parameters.c new file mode 100644 index 0000000000..420b296f86 --- /dev/null +++ b/thirdParty/mavlink/missionlib/mavlink_parameters.c @@ -0,0 +1,146 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +#include "testing/mavlink_missionlib_data.h" +#include "mavlink_parameters.h" +#include "math.h" /* isinf / isnan checks */ + +extern mavlink_system_t mavlink_system; +extern mavlink_pm_storage pm; + +extern void mavlink_missionlib_send_message(mavlink_message_t* msg); +extern void mavlink_missionlib_send_gcs_string(const char* string); + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg) +{ + switch (msg->msgid) + { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { + // Start sending parameters + pm.next_param = 0; + mavlink_missionlib_send_gcs_string("PM SENDING LIST"); + } + break; + case MAVLINK_MSG_ID_PARAM_SET: + { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + // Check if this message is for this system + if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid) + { + char* key = set.param_id; + + for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++) + { + bool match = true; + for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++) + { + // Compare + if (pm.param_names[i][j] != key[j]) + { + match = false; + } + + // End matching if null termination is reached + if (pm.param_names[i][j] == '\0') + { + break; + } + } + + // Check if matched + if (match) + { + // Only write and emit changes if there is actually a difference + // AND only write if new value is NOT "not-a-number" + // AND is NOT infinity + if (pm.param_values[i] != set.param_value + && !isnan(set.param_value) + && !isinf(set.param_value)) + { + pm.param_values[i] = set.param_value; + // Report back new value +#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS + mavlink_message_t tx_msg; + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + pm.param_names[i], + pm.param_values[i], + pm.size, + i); + mavlink_missionlib_send_message(&tx_msg); +#else + mavlink_msg_param_value_send(MAVLINK_COMM_0, + pm.param_names[i], + pm.param_values[i], + pm.size, + i); +#endif + + mavlink_missionlib_send_gcs_string("PM received param"); + } // End valid value check + } // End match check + } // End for loop + } // End system ID check + + } // End case + break; + + } // End switch + +} + + /** + * @brief Send low-priority messages at a maximum rate of xx Hertz + * + * This function sends messages at a lower rate to not exceed the wireless + * bandwidth. It sends one message each time it is called until the buffer is empty. + * Call this function with xx Hertz to increase/decrease the bandwidth. + */ + void mavlink_pm_queued_send(void) + { + //send parameters one by one + if (pm.next_param < pm.size) + { + //for (int i.. all active comm links) +#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS + mavlink_message_t tx_msg; + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + pm.param_names[pm.next_param], + pm.param_values[pm.next_param], + pm.size, + pm.next_param); + mavlink_missionlib_send_message(&tx_msg); +#else + mavlink_msg_param_value_send(MAVLINK_COMM_0, + pm.param_names[pm.next_param], + pm.param_values[pm.next_param], + pm.size, + pm.next_param); +#endif + pm.next_param++; + } + } \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/mavlink_parameters.h b/thirdParty/mavlink/missionlib/mavlink_parameters.h new file mode 100644 index 0000000000..82e7032772 --- /dev/null +++ b/thirdParty/mavlink/missionlib/mavlink_parameters.h @@ -0,0 +1,42 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "mavlink.h" +#include + +/* PARAMETER MANAGER - MISSION LIB */ + +//#ifndef MAVLINK_PM_MAX_PARAM_COUNT +//#error You have not defined the ENUM with your parameters with MAVLINK_PM_MAX_PARAM_COUNT at the end. +//#endif + +#ifndef MAVLINK_PM_TEXT_FEEDBACK +#define MAVLINK_PM_TEXT_FEEDBACK 1 ///< Report back status information as text +#endif + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg); +void mavlink_pm_queued_send(void); \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c index 60b653a798..f90000b1ba 100644 --- a/thirdParty/mavlink/missionlib/testing/main.c +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -52,41 +52,57 @@ #include #endif -#include -<<<<<<< HEAD -#include -======= ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +/* FIRST: MAVLink setup */ +//#define MAVLINK_CONFIGURED +//#define MAVLINK_NO_DATA +#define MAVLINK_WPM_VERBOSE -#include <../mavlink_types.h> +/* 0: Include MAVLink types */ +#include "mavlink_types.h" +/* 1: Define mavlink system storage */ mavlink_system_t mavlink_system; -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ -#include +/* 2: Include actual protocol, REQUIRES mavlink_system */ +#include "mavlink.h" +/* 3: Include MAVLink data structures */ +#include "mavlink_data.h" -<<<<<<< HEAD -======= -/* Provide the interface functions for the waypoint manager */ +/* 3: Define waypoint helper functions */ +void mavlink_wpm_send_message(mavlink_message_t* msg); +void mavlink_wpm_send_gcs_string(const char* string); +uint64_t mavlink_wpm_get_system_timestamp(); +void mavlink_missionlib_send_message(mavlink_message_t* msg); +void mavlink_missionlib_send_gcs_string(const char* string); +uint64_t mavlink_missionlib_get_system_timestamp(); -/* - * @brief Sends a MAVLink message over UDP +/* 4: Include waypoint protocol */ +#include "waypoints.h" +mavlink_wpm_storage wpm; + + +#include "mavlink_missionlib_data.h" +#include "mavlink_parameters.h" + +mavlink_pm_storage pm; + +/** + * @brief reset all parameters to default + * @warning DO NOT USE THIS IN FLIGHT! */ -void mavlink_wpm_send_message(mavlink_message_t* msg) +void mavlink_pm_reset_params(mavlink_pm_storage* pm) { - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - printf("SENT %d bytes", bytes_sent); + pm->size = MAVLINK_PM_MAX_PARAM_COUNT; + // 1) MAVLINK_PM_PARAM_SYSTEM_ID + pm->param_values[MAVLINK_PM_PARAM_SYSTEM_ID] = 12; + strcpy(pm->param_names[MAVLINK_PM_PARAM_SYSTEM_ID], "SYS_ID"); + // 2) MAVLINK_PM_PARAM_ATT_K_D + pm->param_values[MAVLINK_PM_PARAM_ATT_K_D] = 0.3f; + strcpy(pm->param_names[MAVLINK_PM_PARAM_ATT_K_D], "ATT_K_D"); } -#include - - ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) char help[] = "--help"; @@ -110,956 +126,60 @@ unsigned int temp = 0; uint64_t microsSinceEpoch(); -<<<<<<< HEAD -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES -{ - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES -{ - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 100 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 - - -struct mavlink_wpm_storage { - mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t size; - uint16_t max_size; - uint16_t rcv_size; - enum MAVLINK_WPM_STATES current_state; - uint16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; - uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -mavlink_wpm_storage wpm; - -bool debug = true; -bool verbose = true; - - -void mavlink_wpm_init(mavlink_wpm_storage* state) -{ - // Set all waypoints to zero - - // Set count to zero - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - -} +/* Provide the interface functions for the waypoint manager */ /* * @brief Sends a MAVLink message over UDP */ -void mavlink_wpm_send_message(mavlink_message_t* msg) + +void mavlink_missionlib_send_message(mavlink_message_t* msg) { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(buf, msg); uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - printf("SENT %d bytes", bytes_sent); + + printf("SENT %d bytes\n", bytes_sent); } -void mavlink_wpm_send_gcs_string(const char* string) +void mavlink_missionlib_send_gcs_string(const char* string) { - printf("%s",string); + printf("%s\n",string); } -uint64_t mavlink_wpm_get_system_timestamp() +uint64_t mavlink_missionlib_get_system_timestamp() { struct timeval tv; gettimeofday(&tv, NULL); return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; } -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_waypoint_ack_t wpa; - - wpa.target_system = wpm.current_partner_sysid; - wpa.target_component = wpm.current_partner_compid; - wpa.type = type; - - mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); - mavlink_wpm_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) - { - //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); - mavlink_wpm_send_gcs_string("Sent waypoint ACK"); - } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if(seq < wpm.size) - { - mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); - - mavlink_message_t msg; - mavlink_waypoint_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); - mavlink_wpm_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); - } -} -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if(seq < wpm.size) - { - mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); - - mavlink_message_t msg; - mavlink_local_position_setpoint_set_t position_control_set_point; - - // Send new NED or ENU setpoint to onbaord autopilot - if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) - { - position_control_set_point.target_system = mavlink_system.sysid; - position_control_set_point.target_component = MAV_COMP_ID_IMU; - position_control_set_point.x = cur->x; - position_control_set_point.y = cur->y; - position_control_set_point.z = cur->z; - position_control_set_point.yaw = cur->param4; - - mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); - mavlink_wpm_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); - } - - wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_waypoint_count_t wpc; - - wpc.target_system = wpm.current_partner_sysid; - wpc.target_component = wpm.current_partner_compid; - wpc.count = count; - - mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); - mavlink_wpm_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm.size) - { - mavlink_message_t msg; - mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); - wp->target_system = wpm.current_partner_sysid; - wp->target_component = wpm.current_partner_compid; - mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); - mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +void mavlink_wpm_send_message(mavlink_message_t* msg) { - if (seq < wpm.max_size) - { - mavlink_message_t msg; - mavlink_waypoint_request_t wpr; - wpr.target_system = wpm.current_partner_sysid; - wpr.target_component = wpm.current_partner_compid; - wpr.seq = seq; - mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); - mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); - } + mavlink_missionlib_send_message(msg); } -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) +void mavlink_wpm_send_gcs_string(const char* string) { - mavlink_message_t msg; - mavlink_waypoint_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); - mavlink_wpm_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + mavlink_missionlib_send_gcs_string(string); } -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm.size) -// { -// mavlink_waypoint_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm.size) -// { -// mavlink_waypoint_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// if (verbose) printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - -float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +uint64_t mavlink_wpm_get_system_timestamp() { -// if (seq < wpm.size) -// { -// mavlink_waypoint_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// return (C-A).length(); -// } -// else -// { -// if (verbose) printf("ERROR: index out of bounds\n"); -// } - return -1.f; + return mavlink_missionlib_get_system_timestamp(); } -static void mavlink_wpm_message_handler(const mavlink_message_t* msg) -{ - // Handle param messages - //paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - wpm.current_count = 0; - wpm.current_partner_sysid = 0; - wpm.current_partner_compid = 0; - wpm.current_wp_id = -1; - - if(wpm.size == 0) - { - wpm.current_active_wp_id = -1; - } - } - - if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) - { - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - } - - switch(msg->msgid) - { - case MAVLINK_MSG_ID_ATTITUDE: - { - if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) - { - mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); - if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) - { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = wpm.accept_range_yaw; - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) - { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - wpm.yaw_reached = true; - } - else if(att.yaw - yaw_tolerance < 0.0f) - { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - wpm.yaw_reached = true; - } - else - { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - wpm.yaw_reached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION: - { - if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) - { - mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); - - if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) - { - mavlink_local_position_t pos; - mavlink_msg_local_position_decode(msg, &pos); - //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); - - wpm.pos_reached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) - { - // FIXME segment distance - //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); - } - else - { - dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) - { - wpm.pos_reached = true; - } - } - } - break; - } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm.size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - mavlink_waypoint_ack_t wpa; - mavlink_msg_waypoint_ack_decode(msg, &wpa); - - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) - { - if (wpm.current_wp_id == wpm.size-1) - { - if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - wpm.current_wp_id = 0; - } - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - mavlink_waypoint_set_current_t wpc; - mavlink_msg_waypoint_set_current_decode(msg, &wpc); - - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - if (wpc.seq < wpm.size) - { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); - wpm.current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < wpm.size; i++) - { - if (i == wpm.current_active_wp_id) - { - wpm.waypoints[i].current = true; - } - else - { - wpm.waypoints[i].current = false; - } - } - if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); - wpm.yaw_reached = false; - wpm.pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.timestamp_firstinside_orbit = 0; - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - mavlink_waypoint_request_list_t wprl; - mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { - if (wpm.size > 0) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm.current_wp_id = 0; - wpm.current_partner_sysid = msg->sysid; - wpm.current_partner_compid = msg->compid; - } - else - { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - wpm.current_count = wpm.size; - mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); - } - - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - mavlink_waypoint_request_t wpr; - mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm.current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); - } - else - { - if (verbose) - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } - else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } - else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) - { - if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); - else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - mavlink_waypoint_count_t wpc; - mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) - { - if (wpc.count > 0) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_GETLIST; - wpm.current_wp_id = 0; - wpm.current_partner_sysid = msg->sysid; - wpm.current_partner_compid = msg->compid; - wpm.current_count = wpc.count; - - printf("clearing receive buffer and readying for receiving waypoints\n"); - wpm.rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } - - mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); - } - else if (wpc.count == 0) - { - printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); - wpm.rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - break; - - } - else - { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } - else - { - if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); - else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - - } - break; - - case MAVLINK_MSG_ID_WAYPOINT: - { - mavlink_waypoint_t wp; - mavlink_msg_waypoint_decode(msg, &wp); - - if (verbose) printf("GOT WAYPOINT!"); - - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) - { - wpm.timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - - wpm.current_wp_id = wp.seq + 1; - - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - - if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) - { - if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); - - mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - - if (wpm.current_active_wp_id > wpm.rcv_size-1) - { - wpm.current_active_wp_id = wpm.rcv_size-1; - } - - // switch the waypoints list - // FIXME CHECK!!! - for (int i = 0; i < wpm.current_count; ++i) - { - wpm.waypoints[i] = wpm.rcv_waypoints[i]; - } - wpm.size = wpm.current_count; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < wpm.size; i++) - { - if (wpm.waypoints[i].current == 1) - { - wpm.current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - wpm.yaw_reached = false; - wpm.pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == wpm.size) - { - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - wpm.timestamp_firstinside_orbit = 0; - } - - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - } - else - { - mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); - } - } - else - { - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) - { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) - { - if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); - else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); - } - else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - mavlink_waypoint_clear_all_t wpca; - mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - - if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - wpm.timestamp_lastaction = now; - - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm.size = 0; - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - } - else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); - } - break; - } - - default: - { - if (debug) printf("Waypoint: received message of unknown type"); - break; - } - } - - //check if the current waypoint was reached - if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) - { - if (wpm.current_active_wp_id < wpm.size) - { - mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); - - if (wpm.timestamp_firstinside_orbit == 0) - { - // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm.timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) - { - if (cur_wp->autocontinue) - { - cur_wp->current = 0; - if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) - { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - wpm.current_active_wp_id = 1; - } - else - { - if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) - wpm.current_active_wp_id++; - } - - // Fly to next waypoint - wpm.timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.waypoints[wpm.current_active_wp_id].current = true; - wpm.pos_reached = false; - wpm.yaw_reached = false; - if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); - } - } - } - } - else - { - wpm.timestamp_lastoutside_orbit = now; - } -} - - - - - - - -======= ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f int main(int argc, char* argv[]) { // Initialize MAVLink mavlink_wpm_init(&wpm); mavlink_system.sysid = 1; - mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; + mavlink_system.compid = 20; + mavlink_pm_reset_params(&pm); @@ -1131,20 +251,6 @@ 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)); -<<<<<<< HEAD -// /* Send Local Position */ -// mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), -// position[0], position[1], position[2], -// position[3], position[4], position[5]); -// len = mavlink_msg_to_send_buffer(buf, &msg); -// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); -// -// /* Send attitude */ -// mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); -// len = mavlink_msg_to_send_buffer(buf, &msg); -// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); -// -======= /* Send Local Position */ mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], @@ -1157,7 +263,6 @@ 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)); ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f memset(buf, 0, BUFFER_LENGTH); recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); @@ -1181,12 +286,17 @@ int main(int argc, char* argv[]) mavlink_wpm_message_handler(&msg); // Handle packet with parameter component + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } printf("\n"); } memset(buf, 0, BUFFER_LENGTH); - sleep(1); // Sleep one second + usleep(50000); // Sleep one second + + + // Send one parameter + mavlink_pm_queued_send(); } } diff --git a/thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h b/thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h new file mode 100644 index 0000000000..a040941e2c --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h @@ -0,0 +1,54 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "mavlink.h" +#include + +/* MISSION LIB DATA STORAGE */ + +enum MAVLINK_PM_PARAMETERS +{ + MAVLINK_PM_PARAM_SYSTEM_ID, + MAVLINK_PM_PARAM_ATT_K_D, + MAVLINK_PM_MAX_PARAM_COUNT // VERY IMPORTANT! KEEP THIS AS LAST ENTRY +}; + + +/* DO NOT EDIT THIS FILE BELOW THIS POINT! */ + + +//extern void mavlink_pm_queued_send(); +struct mavlink_pm_storage { + char param_names[MAVLINK_PM_MAX_PARAM_COUNT][MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; ///< Parameter names + float param_values[MAVLINK_PM_MAX_PARAM_COUNT]; ///< Parameter values + uint16_t next_param; + uint16_t size; +}; + +typedef struct mavlink_pm_storage mavlink_pm_storage; + +void mavlink_pm_reset_params(mavlink_pm_storage* pm); \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c deleted file mode 100644 index dc096be51b..0000000000 --- a/thirdParty/mavlink/missionlib/testing/udp.c +++ /dev/null @@ -1,1073 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - and Bryan Godbolt godbolt ( a t ) ualberta.ca - - adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ -/* - This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets - cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from - qgroundcontrol are printed by this program along with the heartbeats. - - - I compiled this program sucessfully on Ubuntu 10.04 with the following command - - gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c - - the rt library is needed for the clock_gettime on linux - */ -/* These headers are for QNX, but should all be standard on unix/linux */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if (defined __QNX__) | (defined __QNXNTO__) -/* QNX specific headers */ -#include -#else -/* Linux / MacOS POSIX timer headers */ -#include -#include -#include -#endif - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ -#include - - -#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) - -uint64_t microsSinceEpoch(); - - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES -{ - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES -{ - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 100 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#define MAVLINK_WPM_TEXT_FEEDBACK ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 - -struct _mavlink_wpm_storage { - mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t count; - MAVLINK_WPM_STATES current_state; -} mavlink_wpm_storage; - - -void mavlink_wpm_init(mavlink_wpm_storage* state) -{ - // Set all waypoints to zero - - // Set count to zero - state->count = 0; - state->current_state = MAVLINK_WPM_STATE_IDLE; -} - - -PX_WAYPOINTPLANNER_STATES current_state = PX_WPP_IDLE; -uint16_t protocol_current_wp_id = 0; -uint16_t protocol_current_count = 0; -uint8_t protocol_current_partner_systemid = 0; -uint8_t protocol_current_partner_compid = 0; -uint64_t protocol_timestamp_lastaction = 0; - -uint64_t timestamp_last_send_setpoint = 0; - - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_waypoint_ack_t wpa; - - wpa.target_system = target_systemid; - wpa.target_component = target_compid; - wpa.type = type; - - mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if(seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_waypoint_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if(seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_local_position_setpoint_set_t PControlSetPoint; - - // send new set point to local IMU - if (cur->frame == 1) - { - PControlSetPoint.target_system = systemid; - PControlSetPoint.target_component = MAV_COMP_ID_IMU; - PControlSetPoint.x = cur->x; - PControlSetPoint.y = cur->y; - PControlSetPoint.z = cur->z; - PControlSetPoint.yaw = cur->param4; - - mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); - } - - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - timestamp_last_send_setpoint = now; - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_waypoint_count_t wpc; - - wpc.target_system = target_systemid; - wpc.target_component = target_compid; - wpc.count = count; - - mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) - { - mavlink_message_t msg; - mavlink_waypoint_t *wp = waypoints->at(seq); - wp->target_system = target_systemid; - wp->target_component = target_compid; - mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) - { - mavlink_message_t msg; - mavlink_waypoint_request_t wpr; - wpr.target_system = target_systemid; - wpr.target_component = target_compid; - wpr.seq = seq; - mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_waypoint_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - // seq not the second last waypoint - if ((uint16_t)(seq+1) < waypoints->size()) - { - mavlink_waypoint_t *next = waypoints->at(seq+1); - const PxVector3 B(next->x, next->y, next->z); - const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); - if (r >= 0 && r <= 1) - { - const PxVector3 P(A + r*(B-A)); - return (P-C).length(); - } - else if (r < 0.f) - { - return (C-A).length(); - } - else - { - return (C-B).length(); - } - } - else - { - return (C-A).length(); - } - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } - return -1.f; -} - -float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - return (C-A).length(); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } - return -1.f; -} - - -static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user) -{ - // Handle param messages - paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); - current_state = PX_WPP_IDLE; - protocol_current_count = 0; - protocol_current_partner_systemid = 0; - protocol_current_partner_compid = 0; - protocol_current_wp_id = -1; - - if(waypoints->size() == 0) - { - current_active_wp_id = -1; - } - } - - if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size()) - { - send_setpoint(current_active_wp_id); - } - - switch(msg->msgid) - { - case MAVLINK_MSG_ID_ATTITUDE: - { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); - if(wp->frame == 1) - { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE"); - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) - { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - yawReached = true; - } - else if(att.yaw - yaw_tolerance < 0.0f) - { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - yawReached = true; - } - else - { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - yawReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION: - { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 1) - { - mavlink_local_position_t pos; - mavlink_msg_local_position_decode(msg, &pos); - if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); - - posReached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) - { - dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); - } - else - { - dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && yawReached) - { - posReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_CMD: // special action from ground station - { - mavlink_cmd_t action; - mavlink_msg_cmd_decode(msg, &action); - if(action.target == systemid) - { - if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; - switch (action.action) - { - // case MAV_ACTION_LAUNCH: - // if (verbose) std::cerr << "Launch received" << std::endl; - // current_active_wp_id = 0; - // if (waypoints->size()>0) - // { - // setActive(waypoints[current_active_wp_id]); - // } - // else - // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; - // break; - - // case MAV_ACTION_CONTINUE: - // if (verbose) std::c - // err << "Continue received" << std::endl; - // idle = false; - // setActive(waypoints[current_active_wp_id]); - // break; - - // case MAV_ACTION_HALT: - // if (verbose) std::cerr << "Halt received" << std::endl; - // idle = true; - // break; - - // default: - // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; - // break; - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - mavlink_waypoint_ack_t wpa; - mavlink_msg_waypoint_ack_decode(msg, &wpa); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) - { - if (protocol_current_wp_id == waypoints->size()-1) - { - if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); - current_state = PX_WPP_IDLE; - protocol_current_wp_id = 0; - } - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - mavlink_waypoint_set_current_t wpc; - mavlink_msg_waypoint_set_current_decode(msg, &wpc); - - if(wpc.target_system == systemid && wpc.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE) - { - if (wpc.seq < waypoints->size()) - { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); - current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < waypoints->size(); i++) - { - if (i == current_active_wp_id) - { - waypoints->at(i)->current = true; - } - else - { - waypoints->at(i)->current = false; - } - } - if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); - } - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - mavlink_waypoint_request_list_t wprl; - mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == systemid && wprl.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) - { - if (waypoints->size() > 0) - { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); - current_state = PX_WPP_SENDLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - } - else - { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - protocol_current_count = waypoints->size(); - send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - mavlink_waypoint_request_t wpr; - mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) - { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) - { - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - current_state = PX_WPP_SENDLIST_SENDWPS; - protocol_current_wp_id = wpr.seq; - send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); - } - else - { - if (verbose) - { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } - else if (current_state == PX_WPP_SENDLIST) - { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } - else if (current_state == PX_WPP_SENDLIST_SENDWPS) - { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - mavlink_waypoint_count_t wpc; - mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == systemid && wpc.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) - { - if (wpc.count > 0) - { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - current_state = PX_WPP_GETLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - protocol_current_count = wpc.count; - - printf("clearing receive buffer and readying for receiving waypoints\n"); - while(waypoints_receive_buffer->size() > 0) - { - delete waypoints_receive_buffer->back(); - waypoints_receive_buffer->pop_back(); - } - - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - else if (wpc.count == 0) - { - printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n"); - while(waypoints_receive_buffer->size() > 0) - { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - yawReached = false; - posReached = false; - break; - - } - else - { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } - else - { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT: - { - mavlink_waypoint_t wp; - mavlink_msg_waypoint_decode(msg, &wp); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) - { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) - { - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - - current_state = PX_WPP_GETLIST_GETWPS; - protocol_current_wp_id = wp.seq + 1; - mavlink_waypoint_t* newwp = new mavlink_waypoint_t; - memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - waypoints_receive_buffer->push_back(newwp); - - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - - if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) - { - if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); - - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - - if (current_active_wp_id > waypoints_receive_buffer->size()-1) - { - current_active_wp_id = waypoints_receive_buffer->size() - 1; - } - - // switch the waypoints list - std::vector* waypoints_temp = waypoints; - waypoints = waypoints_receive_buffer; - waypoints_receive_buffer = waypoints_temp; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < waypoints->size(); i++) - { - if (waypoints->at(i)->current == 1) - { - current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == waypoints->size()) - { - current_active_wp_id = -1; - yawReached = false; - posReached = false; - timestamp_firstinside_orbit = 0; - } - - current_state = PX_WPP_IDLE; - } - else - { - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - } - else - { - if (current_state == PX_WPP_IDLE) - { - //we're done receiving waypoints, answer with ack. - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) - { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } - else if (current_state == PX_WPP_GETLIST) - { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else if (current_state == PX_WPP_GETLIST_GETWPS) - { - if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); - } - else if(wp.target_system == systemid && wp.target_component == compid) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - mavlink_waypoint_clear_all_t wpca; - mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - - if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) - { - protocol_timestamp_lastaction = now; - - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - while(waypoints->size() > 0) - { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - yawReached = false; - posReached = false; - } - else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); - } - break; - } - - default: - { - if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl; - break; - } - } - - //check if the current waypoint was reached - if ((posReached && /*yawReached &&*/ !idle)) - { - if (current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); - - if (timestamp_firstinside_orbit == 0) - { - // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); - send_waypoint_reached(cur_wp->seq); - timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) - { - if (cur_wp->autocontinue) - { - cur_wp->current = 0; - if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) - { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - current_active_wp_id = 1; - } - else - { - if ((uint16_t)(current_active_wp_id + 1) < waypoints->size()) - current_active_wp_id++; - } - - // Fly to next waypoint - timestamp_firstinside_orbit = 0; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - waypoints->at(current_active_wp_id)->current = true; - posReached = false; - yawReached = false; - if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); - } - } - } - } - else - { - timestamp_lastoutside_orbit = now; - } -} - - - - - - - - -int main(int argc, char* argv[]) -{ - - char help[] = "--help"; - - - char target_ip[100]; - - float position[6] = {}; - int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - struct sockaddr_in gcAddr; - struct sockaddr_in locAddr; - //struct sockaddr_in fromAddr; - uint8_t buf[BUFFER_LENGTH]; - ssize_t recsize; - socklen_t fromlen; - int bytes_sent; - mavlink_message_t msg; - uint16_t len; - int i = 0; - //int success = 0; - unsigned int temp = 0; - - // Check if --help flag was used - if ((argc == 2) && (strcmp(argv[1], help) == 0)) - { - printf("\n"); - printf("\tUsage:\n\n"); - printf("\t"); - printf("%s", argv[0]); - printf(" \n"); - printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); - exit(EXIT_FAILURE); - } - - - // Change the target ip if parameter was given - strcpy(target_ip, "127.0.0.1"); - if (argc == 2) - { - strcpy(target_ip, argv[1]); - } - - - memset(&locAddr, 0, sizeof(locAddr)); - locAddr.sin_family = AF_INET; - locAddr.sin_addr.s_addr = INADDR_ANY; - locAddr.sin_port = htons(14551); - - /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ - if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) - { - perror("error bind failed"); - close(sock); - exit(EXIT_FAILURE); - } - - /* Attempt to make it non blocking */ - if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) - { - fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); - close(sock); - exit(EXIT_FAILURE); - } - - - memset(&gcAddr, 0, sizeof(gcAddr)); - gcAddr.sin_family = AF_INET; - gcAddr.sin_addr.s_addr = inet_addr(target_ip); - gcAddr.sin_port = htons(14550); - - - printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); - - - for (;;) - { - - /*Send Heartbeat */ - mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send Status */ - mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - /* Send Local Position */ - mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), - position[0], position[1], position[2], - position[3], position[4], position[5]); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send attitude */ - mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - - memset(buf, 0, BUFFER_LENGTH); - recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); - if (recsize > 0) - { - // Something received - print out all bytes and parse packet - mavlink_message_t msg; - mavlink_status_t status; - - printf("Bytes Received: %d\nDatagram: ", (int)recsize); - for (i = 0; i < recsize; ++i) - { - temp = buf[i]; - printf("%02x ", (unsigned char)temp); - if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) - { - // Packet received - printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); - } - } - printf("\n"); - } - memset(buf, 0, BUFFER_LENGTH); - sleep(1); // Sleep one second - } -} - - -/* QNX timer version */ -#if (defined __QNX__) | (defined __QNXNTO__) -uint64_t microsSinceEpoch() -{ - - struct timespec time; - - uint64_t micros = 0; - - clock_gettime(CLOCK_REALTIME, &time); - micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; - - return micros; -} -#else -uint64_t microsSinceEpoch() -{ - - struct timeval tv; - - uint64_t micros = 0; - - gettimeofday(&tv, NULL); - micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - - return micros; -} -#endif \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c index d0e7621f75..552fb12f51 100644 --- a/thirdParty/mavlink/missionlib/waypoints.c +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -17,7 +17,7 @@ ****************************************************************************/ -#include +#include "waypoints.h" #include bool debug = true; @@ -33,6 +33,8 @@ extern uint64_t mavlink_wpm_get_system_timestamp(); #define MAVLINK_WPM_NO_PRINTF +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_WAYPOINTPLANNER; + void mavlink_wpm_init(mavlink_wpm_storage* state) { // Set all waypoints to zero @@ -68,7 +70,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) wpa.target_component = wpm.current_partner_compid; wpa.type = type; - mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); mavlink_wpm_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); @@ -104,7 +106,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = cur->seq; - mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_wpm_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); @@ -144,7 +146,7 @@ void mavlink_wpm_send_setpoint(uint16_t seq) position_control_set_point.z = cur->z; position_control_set_point.yaw = cur->param4; - mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &position_control_set_point); mavlink_wpm_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); @@ -171,7 +173,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou wpc.target_component = wpm.current_partner_compid; wpc.count = count; - mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); @@ -187,7 +189,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); wp->target_system = wpm.current_partner_sysid; wp->target_component = wpm.current_partner_compid; - mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); @@ -208,7 +210,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s wpr.target_system = wpm.current_partner_sysid; wpr.target_component = wpm.current_partner_compid; wpr.seq = seq; - mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); @@ -234,7 +236,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); @@ -450,7 +452,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_ack_t wpa; mavlink_msg_waypoint_ack_decode(msg, &wpa); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { wpm.timestamp_lastaction = now; @@ -484,7 +486,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_set_current_t wpc; mavlink_msg_waypoint_set_current_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/) + if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -550,7 +552,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_list_t wprl; mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/) + if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -558,8 +560,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpm.size > 0) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + //if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; wpm.current_wp_id = 0; wpm.current_partner_sysid = msg->sysid; @@ -589,7 +591,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_t wpr; mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -682,7 +684,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) else { //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); @@ -707,7 +709,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_count_t wpc; mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/) + if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -825,29 +827,29 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_t wp; mavlink_msg_waypoint_decode(msg, &wp); - // if (verbose) // printf("GOT WAYPOINT!"); + mavlink_wpm_send_gcs_string("GOT WP"); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); +// wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - - wpm.current_wp_id = wp.seq + 1; + wpm.current_wp_id = wp.seq + 1; // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + mavlink_wpm_send_gcs_string("GOT ALL WPS"); // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); @@ -948,11 +950,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) else { //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); } - else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/) + else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } @@ -965,7 +967,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_clear_all_t wpca; mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) { wpm.timestamp_lastaction = now; @@ -976,7 +978,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.yaw_reached = false; wpm.pos_reached = false; } - else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); } diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h index 2c3416a821..a0f2aa6d17 100644 --- a/thirdParty/mavlink/missionlib/waypoints.h +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -19,6 +19,12 @@ /* This assumes you have the mavlink headers on your include path or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + #include #include @@ -47,11 +53,9 @@ enum MAVLINK_WPM_CODES /* WAYPOINT MANAGER - MISSION LIB */ -#define MAVLINK_WPM_MAX_WP_COUNT 30 +#define MAVLINK_WPM_MAX_WP_COUNT 15 #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates #define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 #define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 #define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 -- GitLab From 1f23f9cb2e3add4a6717f7a2949b7135f2239f42 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 13 Aug 2011 09:29:14 +0200 Subject: [PATCH 024/131] Adjusted release version --- src/configuration.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/configuration.h b/src/configuration.h index 59ef6a327e..ca911ebeb9 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -12,14 +12,14 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 0.9.0 (Alpha RC1)" +#define QGC_APPLICATION_VERSION "v. 1.0.0 (Alpha RC1)" namespace QGC { const QString APPNAME = "QGROUNDCONTROL"; const QString COMPANYNAME = "QGROUNDCONTROL"; -const int APPLICATIONVERSION = 90; // 0.9.0 +const int APPLICATIONVERSION = 100; // 1.0.0 } #endif // QGC_CONFIGURATION_H -- GitLab From 5fcd18eb4d8d350f67034937ccff8830a1cc6462 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 13 Aug 2011 14:11:08 +0200 Subject: [PATCH 025/131] Ported a larger portion to MAVLink v1.0 --- src/uas/UAS.cc | 73 ++++++++++++++++++++------------------------------ src/uas/UAS.h | 1 - 2 files changed, 29 insertions(+), 45 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1c1a06b2fc..95d003c4b0 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1216,58 +1216,49 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) } void UAS::startRadioControlCalibration() -{// FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_DO_RC_CALIB; -// sendMessage(msg); -} - -void UAS::startDataRecording() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); -// sendMessage(msg); + mavlink_message_t msg; + // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 1); + sendMessage(msg); } -void UAS::pauseDataRecording() +void UAS::startDataRecording() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); -// sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 2); + sendMessage(msg); } void UAS::stopDataRecording() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); -// sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0); + sendMessage(msg); } void UAS::startMagnetometerCalibration() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); -// sendMessage(msg); + mavlink_message_t msg; + // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0); + sendMessage(msg); } void UAS::startGyroscopeCalibration() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); -// sendMessage(msg); + mavlink_message_t msg; + // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0); + sendMessage(msg); } void UAS::startPressureCalibration() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); -// sendMessage(msg); + mavlink_message_t msg; + // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0); + sendMessage(msg); } quint64 UAS::getUnixReferenceTime(quint64 time) @@ -1658,27 +1649,21 @@ void UAS::requestParameters() { mavlink_message_t msg; mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 25); - // Send message twice to increase chance of reception sendMessage(msg); } void UAS::writeParametersToStorage() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); -// //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); -// sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1); + sendMessage(msg); } void UAS::readParametersFromStorage() { - // FIXME MAVLINKV10PORTINGNEEDED -// mavlink_message_t msg; -// // TODO Replace MG System ID with static function call and allow to change ID in GUI -// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); -// sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_command_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1); + sendMessage(msg); } void UAS::enableAllDataTransmission(int rate) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 32703e8eb2..f8121d22fd 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -380,7 +380,6 @@ public slots: void startPressureCalibration(); void startDataRecording(); - void pauseDataRecording(); void stopDataRecording(); signals: -- GitLab From 8f129883ea2ebcfdd6372a20102a7ded4457bbb8 Mon Sep 17 00:00:00 2001 From: lm Date: Sun, 14 Aug 2011 22:03:48 +0200 Subject: [PATCH 026/131] Pushed HIL further --- flightgear/Malolo1/.#Malolo1-set.xml.1.1 | 103 + flightgear/Malolo1/COPYING | 340 + flightgear/Malolo1/Engines/18x8.xml | 51 + flightgear/Malolo1/Engines/Zenoah_G-26A.xml | 8 + flightgear/Malolo1/Malolo1-set.xml | 103 + flightgear/Malolo1/Malolo1-splash.rgb | Bin 0 -> 795671 bytes flightgear/Malolo1/Malolo1.xml | 546 + flightgear/Malolo1/Models/DSC01758.rgb | Bin 0 -> 143335 bytes flightgear/Malolo1/Models/Malolo1.ac | 4197 + flightgear/Malolo1/Models/Malolo1.xml | 103 + flightgear/Malolo1/thumbnail.jpg | Bin 0 -> 65591 bytes flightgear/Rascal110/Dialogs/config.xml | 92 + flightgear/Rascal110/Engines/18x8.xml | 51 + flightgear/Rascal110/Engines/Zenoah_G-26A.xml | 8 + flightgear/Rascal110/Models/Rascal.rgb | Bin 0 -> 159167 bytes .../Rascal110/Models/Rascal110-000-013.ac | 22017 +++++ .../Rascal110-000-013.ac.before-color-change | 22017 +++++ flightgear/Rascal110/Models/Rascal110.xml | 87 + .../Rascal110/Models/Trajectory-Marker.ac | 30 + .../Rascal110/Models/Trajectory-Marker.xml | 9 + flightgear/Rascal110/Models/fix.sh | 8 + flightgear/Rascal110/Models/smoke.png | Bin 0 -> 15427 bytes flightgear/Rascal110/Models/smokeW.xml | 102 + flightgear/Rascal110/README.Rascal | 236 + flightgear/Rascal110/Rascal-keyboard.xml | 24 + flightgear/Rascal110/Rascal-submodels.xml | 25 + flightgear/Rascal110/Rascal110-JSBSim-set.xml | 213 + flightgear/Rascal110/Rascal110-JSBSim.xml | 522 + flightgear/Rascal110/Rascal110-YASim-set.xml | 108 + flightgear/Rascal110/Rascal110-YASim.xml | 129 + flightgear/Rascal110/Rascal110-splash.rgb | Bin 0 -> 768035 bytes .../Rascal110/Systems/.#110-autopilot.xml.1.2 | 711 + .../Rascal110/Systems/110-autopilot.xml | 767 + .../Rascal110/Systems/110-autopilot.xml-a | 707 + flightgear/Rascal110/Systems/airdata.nas | 40 + flightgear/Rascal110/Systems/electrical.xml | 140 + flightgear/Rascal110/Systems/main.nas | 22 + flightgear/Rascal110/Systems/ugear.nas | 54 + flightgear/Rascal110/initfile.xml | 12 + flightgear/Rascal110/thumbnail.jpg | Bin 0 -> 6521 bytes flightgear/YardStick/Engines/18x8.xml | 51 + flightgear/YardStick/Engines/Zenoah_G-26A.xml | 8 + flightgear/YardStick/Models/yardstik.ac | 11844 +++ flightgear/YardStick/Models/yardstik.xml | 106 + flightgear/YardStick/YardStik-set.xml | 102 + flightgear/YardStick/YardStik-splash.rgb | Bin 0 -> 3056689 bytes flightgear/YardStick/YardStik.xml | 522 + flightgear/YardStick/thumbnail.jpg | Bin 0 -> 6521 bytes flightgear/ardupilot.xml | 104 + .../jeep/Models/AI-Liveries/airborn.xml | 14 + flightgear/jeep/Models/AI-Liveries/army-g.xml | 14 + flightgear/jeep/Models/AI-Liveries/hippie.xml | 14 + flightgear/jeep/Models/AI-Liveries/jeep-1.png | Bin 0 -> 111066 bytes .../jeep/Models/AI-Liveries/jeep-1a.png | Bin 0 -> 354116 bytes .../jeep/Models/AI-Liveries/jeep-1b.png | Bin 0 -> 252333 bytes .../jeep/Models/AI-Liveries/jeep-1c.png | Bin 0 -> 130466 bytes .../jeep/Models/AI-Liveries/jeep-1d.png | Bin 0 -> 64986 bytes flightgear/jeep/Models/AI-Liveries/navy.xml | 14 + flightgear/jeep/Models/AI-Liveries/raf.xml | 14 + .../jeep/Models/AI-Liveries/softroof-a.png | Bin 0 -> 47942 bytes .../jeep/Models/AI-Liveries/softroof.png | Bin 0 -> 49463 bytes flightgear/jeep/Models/GPA.xml | 320 + flightgear/jeep/Models/Jeep.xml | 540 + flightgear/jeep/Models/Liveries/airborn.xml | 14 + flightgear/jeep/Models/Liveries/army-g.xml | 14 + flightgear/jeep/Models/Liveries/hippie.xml | 14 + flightgear/jeep/Models/Liveries/jeep-1a.png | Bin 0 -> 1349925 bytes flightgear/jeep/Models/Liveries/jeep-1b.png | Bin 0 -> 889097 bytes flightgear/jeep/Models/Liveries/jeep-1c.png | Bin 0 -> 2311967 bytes flightgear/jeep/Models/Liveries/jeep-1d.png | Bin 0 -> 886539 bytes flightgear/jeep/Models/Liveries/navy.xml | 14 + flightgear/jeep/Models/Liveries/raf.xml | 14 + .../jeep/Models/Liveries/softroof-a.png | Bin 0 -> 186109 bytes flightgear/jeep/Models/cabin.png | Bin 0 -> 236932 bytes flightgear/jeep/Models/current.ac | 1221 + flightgear/jeep/Models/current.rgb | Bin 0 -> 16244 bytes flightgear/jeep/Models/current.xml | 29 + flightgear/jeep/Models/fuel.ac | 1215 + flightgear/jeep/Models/fuel.rgb | Bin 0 -> 15033 bytes flightgear/jeep/Models/fuelgauge.xml | 29 + flightgear/jeep/Models/gpa.ac | 51710 ++++++++++++ flightgear/jeep/Models/gpatrans.ac | 35 + flightgear/jeep/Models/gpatrans.xml | 29 + flightgear/jeep/Models/jeep-1.png | Bin 0 -> 1668757 bytes flightgear/jeep/Models/jeep-2.png | Bin 0 -> 337564 bytes flightgear/jeep/Models/jeep.ac | 66674 ++++++++++++++++ flightgear/jeep/Models/pilot-b.ac | 19379 +++++ flightgear/jeep/Models/pilot.xml | 272 + flightgear/jeep/Models/pilot1.rgb | Bin 0 -> 63852 bytes flightgear/jeep/Models/roof.ac | 2850 + flightgear/jeep/Models/softroof.png | Bin 0 -> 205063 bytes flightgear/jeep/Models/speedometer.ac | 1718 + flightgear/jeep/Models/speedometer.rgb | Bin 0 -> 23438 bytes flightgear/jeep/Models/speedometer.xml | 29 + flightgear/jeep/Models/transparent.ac | 35 + flightgear/jeep/Models/transparent.xml | 29 + flightgear/jeep/Nasal/failure.nas | 10 + flightgear/jeep/Nasal/jeep.nas | 1 + flightgear/jeep/Nasal/walk.nas | 101 + flightgear/jeep/README | 15 + flightgear/jeep/gpa-set.xml | 239 + flightgear/jeep/gpa-yasim.xml | 94 + flightgear/jeep/jeep-set.xml | 269 + flightgear/jeep/jeep-yasim.xml | 77 + flightgear/jeep/thumbnail.jpg | Bin 0 -> 3050 bytes src/comm/MAVLinkSimulationLink.cc | 2 +- src/comm/QGCFlightGearLink.cc | 209 +- src/comm/QGCFlightGearLink.h | 16 +- src/uas/UAS.cc | 54 +- src/uas/UAS.h | 14 + src/ui/MainWindow.cc | 5 - src/ui/uas/UASView.cc | 4 + src/ui/uas/UASView.h | 1 + 113 files changed, 213611 insertions(+), 73 deletions(-) create mode 100644 flightgear/Malolo1/.#Malolo1-set.xml.1.1 create mode 100644 flightgear/Malolo1/COPYING create mode 100644 flightgear/Malolo1/Engines/18x8.xml create mode 100644 flightgear/Malolo1/Engines/Zenoah_G-26A.xml create mode 100644 flightgear/Malolo1/Malolo1-set.xml create mode 100644 flightgear/Malolo1/Malolo1-splash.rgb create mode 100644 flightgear/Malolo1/Malolo1.xml create mode 100644 flightgear/Malolo1/Models/DSC01758.rgb create mode 100644 flightgear/Malolo1/Models/Malolo1.ac create mode 100644 flightgear/Malolo1/Models/Malolo1.xml create mode 100644 flightgear/Malolo1/thumbnail.jpg create mode 100644 flightgear/Rascal110/Dialogs/config.xml create mode 100644 flightgear/Rascal110/Engines/18x8.xml create mode 100644 flightgear/Rascal110/Engines/Zenoah_G-26A.xml create mode 100755 flightgear/Rascal110/Models/Rascal.rgb create mode 100755 flightgear/Rascal110/Models/Rascal110-000-013.ac create mode 100755 flightgear/Rascal110/Models/Rascal110-000-013.ac.before-color-change create mode 100644 flightgear/Rascal110/Models/Rascal110.xml create mode 100644 flightgear/Rascal110/Models/Trajectory-Marker.ac create mode 100644 flightgear/Rascal110/Models/Trajectory-Marker.xml create mode 100755 flightgear/Rascal110/Models/fix.sh create mode 100644 flightgear/Rascal110/Models/smoke.png create mode 100644 flightgear/Rascal110/Models/smokeW.xml create mode 100644 flightgear/Rascal110/README.Rascal create mode 100644 flightgear/Rascal110/Rascal-keyboard.xml create mode 100644 flightgear/Rascal110/Rascal-submodels.xml create mode 100644 flightgear/Rascal110/Rascal110-JSBSim-set.xml create mode 100644 flightgear/Rascal110/Rascal110-JSBSim.xml create mode 100644 flightgear/Rascal110/Rascal110-YASim-set.xml create mode 100644 flightgear/Rascal110/Rascal110-YASim.xml create mode 100644 flightgear/Rascal110/Rascal110-splash.rgb create mode 100644 flightgear/Rascal110/Systems/.#110-autopilot.xml.1.2 create mode 100644 flightgear/Rascal110/Systems/110-autopilot.xml create mode 100644 flightgear/Rascal110/Systems/110-autopilot.xml-a create mode 100644 flightgear/Rascal110/Systems/airdata.nas create mode 100644 flightgear/Rascal110/Systems/electrical.xml create mode 100644 flightgear/Rascal110/Systems/main.nas create mode 100644 flightgear/Rascal110/Systems/ugear.nas create mode 100644 flightgear/Rascal110/initfile.xml create mode 100644 flightgear/Rascal110/thumbnail.jpg create mode 100644 flightgear/YardStick/Engines/18x8.xml create mode 100644 flightgear/YardStick/Engines/Zenoah_G-26A.xml create mode 100644 flightgear/YardStick/Models/yardstik.ac create mode 100644 flightgear/YardStick/Models/yardstik.xml create mode 100644 flightgear/YardStick/YardStik-set.xml create mode 100644 flightgear/YardStick/YardStik-splash.rgb create mode 100644 flightgear/YardStick/YardStik.xml create mode 100644 flightgear/YardStick/thumbnail.jpg create mode 100644 flightgear/ardupilot.xml create mode 100755 flightgear/jeep/Models/AI-Liveries/airborn.xml create mode 100755 flightgear/jeep/Models/AI-Liveries/army-g.xml create mode 100755 flightgear/jeep/Models/AI-Liveries/hippie.xml create mode 100755 flightgear/jeep/Models/AI-Liveries/jeep-1.png create mode 100755 flightgear/jeep/Models/AI-Liveries/jeep-1a.png create mode 100755 flightgear/jeep/Models/AI-Liveries/jeep-1b.png create mode 100755 flightgear/jeep/Models/AI-Liveries/jeep-1c.png create mode 100755 flightgear/jeep/Models/AI-Liveries/jeep-1d.png create mode 100755 flightgear/jeep/Models/AI-Liveries/navy.xml create mode 100755 flightgear/jeep/Models/AI-Liveries/raf.xml create mode 100755 flightgear/jeep/Models/AI-Liveries/softroof-a.png create mode 100755 flightgear/jeep/Models/AI-Liveries/softroof.png create mode 100644 flightgear/jeep/Models/GPA.xml create mode 100755 flightgear/jeep/Models/Jeep.xml create mode 100755 flightgear/jeep/Models/Liveries/airborn.xml create mode 100755 flightgear/jeep/Models/Liveries/army-g.xml create mode 100755 flightgear/jeep/Models/Liveries/hippie.xml create mode 100755 flightgear/jeep/Models/Liveries/jeep-1a.png create mode 100755 flightgear/jeep/Models/Liveries/jeep-1b.png create mode 100755 flightgear/jeep/Models/Liveries/jeep-1c.png create mode 100755 flightgear/jeep/Models/Liveries/jeep-1d.png create mode 100755 flightgear/jeep/Models/Liveries/navy.xml create mode 100755 flightgear/jeep/Models/Liveries/raf.xml create mode 100755 flightgear/jeep/Models/Liveries/softroof-a.png create mode 100755 flightgear/jeep/Models/cabin.png create mode 100755 flightgear/jeep/Models/current.ac create mode 100755 flightgear/jeep/Models/current.rgb create mode 100755 flightgear/jeep/Models/current.xml create mode 100755 flightgear/jeep/Models/fuel.ac create mode 100755 flightgear/jeep/Models/fuel.rgb create mode 100755 flightgear/jeep/Models/fuelgauge.xml create mode 100644 flightgear/jeep/Models/gpa.ac create mode 100644 flightgear/jeep/Models/gpatrans.ac create mode 100644 flightgear/jeep/Models/gpatrans.xml create mode 100755 flightgear/jeep/Models/jeep-1.png create mode 100755 flightgear/jeep/Models/jeep-2.png create mode 100755 flightgear/jeep/Models/jeep.ac create mode 100755 flightgear/jeep/Models/pilot-b.ac create mode 100755 flightgear/jeep/Models/pilot.xml create mode 100755 flightgear/jeep/Models/pilot1.rgb create mode 100755 flightgear/jeep/Models/roof.ac create mode 100755 flightgear/jeep/Models/softroof.png create mode 100755 flightgear/jeep/Models/speedometer.ac create mode 100755 flightgear/jeep/Models/speedometer.rgb create mode 100755 flightgear/jeep/Models/speedometer.xml create mode 100755 flightgear/jeep/Models/transparent.ac create mode 100755 flightgear/jeep/Models/transparent.xml create mode 100755 flightgear/jeep/Nasal/failure.nas create mode 100755 flightgear/jeep/Nasal/jeep.nas create mode 100755 flightgear/jeep/Nasal/walk.nas create mode 100755 flightgear/jeep/README create mode 100644 flightgear/jeep/gpa-set.xml create mode 100644 flightgear/jeep/gpa-yasim.xml create mode 100755 flightgear/jeep/jeep-set.xml create mode 100755 flightgear/jeep/jeep-yasim.xml create mode 100755 flightgear/jeep/thumbnail.jpg diff --git a/flightgear/Malolo1/.#Malolo1-set.xml.1.1 b/flightgear/Malolo1/.#Malolo1-set.xml.1.1 new file mode 100644 index 0000000000..c85509bb87 --- /dev/null +++ b/flightgear/Malolo1/.#Malolo1-set.xml.1.1 @@ -0,0 +1,103 @@ + + + + + + + Malolo1(R/C) + Innis Cunningham, Josh Wilson + 0.0 + + + Aircraft/Malolo1/Malolo1-splash.rgb + + + jsb + Malolo1 + 0.8 + + + + Aircraft/Generic/generic-sound.xml + + + + false + + + + Aircraft/Malolo1/Models/Malolo1.xml + + + + true + + 0.0 + 0.26 + 0.34 + -8 + + + + -15.0 + + YardStik 110 (Sig Mfg) + Cruise speed: 60 mph + Never-exceed (Vne): 85 mph + Best Glide (Vglide): 20 mph + Maneuvering (Va): 50 mph + Approach speed: 15-25 mph + Stall speed (Vs): 10 mph + + + + + + + -0.01 + 0.00 + 0.00 + + + + 3 + + + 1.0 + + + + + 700 + + + + + + + 48.0 + 56.0 + 400.0 + 1000.0 + 6000.0 + 0.008 + 0.35 + 0.001 + + + + diff --git a/flightgear/Malolo1/COPYING b/flightgear/Malolo1/COPYING new file mode 100644 index 0000000000..d60c31a97a --- /dev/null +++ b/flightgear/Malolo1/COPYING @@ -0,0 +1,340 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Library General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Library General +Public License instead of this License. diff --git a/flightgear/Malolo1/Engines/18x8.xml b/flightgear/Malolo1/Engines/18x8.xml new file mode 100644 index 0000000000..6f4aae8cf6 --- /dev/null +++ b/flightgear/Malolo1/Engines/18x8.xml @@ -0,0 +1,51 @@ + + + + + + 0.00085 + 18.0 + 2 + 30 + 30 + + + + 0.0 0.0776 + 0.1 0.0744 + 0.2 0.0712 + 0.3 0.0655 + 0.4 0.0588 + 0.5 0.0518 + 0.6 0.0419 + 0.7 0.0318 + 0.8 0.0172 + 1.0 -0.0058 + 1.4 -0.0549 + +
+ + + + 0.0 0.0902 + 0.1 0.0893 + 0.2 0.0880 + 0.3 0.0860 + 0.4 0.0810 + 0.5 0.0742 + 0.6 0.0681 + 0.7 0.0572 + 0.8 0.0467 + 1.0 0.0167 + 1.4 -0.0803 + +
+ + diff --git a/flightgear/Malolo1/Engines/Zenoah_G-26A.xml b/flightgear/Malolo1/Engines/Zenoah_G-26A.xml new file mode 100644 index 0000000000..86d43a827e --- /dev/null +++ b/flightgear/Malolo1/Engines/Zenoah_G-26A.xml @@ -0,0 +1,8 @@ + + + + + + + 2207.27 + diff --git a/flightgear/Malolo1/Malolo1-set.xml b/flightgear/Malolo1/Malolo1-set.xml new file mode 100644 index 0000000000..c85509bb87 --- /dev/null +++ b/flightgear/Malolo1/Malolo1-set.xml @@ -0,0 +1,103 @@ + + + + + + + Malolo1(R/C) + Innis Cunningham, Josh Wilson + 0.0 + + + Aircraft/Malolo1/Malolo1-splash.rgb + + + jsb + Malolo1 + 0.8 + + + + Aircraft/Generic/generic-sound.xml + + + + false + + + + Aircraft/Malolo1/Models/Malolo1.xml + + + + true + + 0.0 + 0.26 + 0.34 + -8 + + + + -15.0 + + YardStik 110 (Sig Mfg) + Cruise speed: 60 mph + Never-exceed (Vne): 85 mph + Best Glide (Vglide): 20 mph + Maneuvering (Va): 50 mph + Approach speed: 15-25 mph + Stall speed (Vs): 10 mph + + + + + + + -0.01 + 0.00 + 0.00 + + + + 3 + + + 1.0 + + + + + 700 + + + + + + + 48.0 + 56.0 + 400.0 + 1000.0 + 6000.0 + 0.008 + 0.35 + 0.001 + + + + diff --git a/flightgear/Malolo1/Malolo1-splash.rgb b/flightgear/Malolo1/Malolo1-splash.rgb new file mode 100644 index 0000000000000000000000000000000000000000..02f47ac2ed3c920a63e3b9bb69851102b4e13bd4 GIT binary patch literal 795671 zcmeFZhodBAvG-pyJ#)^SnH5D*5D~eEh={0&B$11VsL16aB61M{3Cpr98&A%YbIv)O zxRC{kD@i0t5&;Df5fKnk5fMQV*59X^GyCrS{R{6a_I6MA(@(6Y>RaEcK9(mf7BQ@1 z6*1(U|NVRE{Qpk>H_X7Y{r8n+`+QWEz4vpnY}s+LtmiATOui(pH%}MWYiEh;xhcX1u} z7jYfQari6ZI^=KSI`D0AeYj6td+#HzJwGTe*OB6~pD50Me^s3SJWHH^`<^)ee1$lF zKPt}O2E_TRusDC76X#E7#QBqXasK!^ao%#PIB$4BoLB!soST0p&N;4|4~nz=kT|oy z5@-B3;tc*pobJDhbL?$#4jSTI`z~>=Tqe#-J|NEXKP=93J}%BPKO@f5juq!Aw0+`# ziSszxKk`y>9)6`b51A6@fo^f`7Zc|PisF1vO`Og}aoXmjZ|6qo`v=FXT>oiS`d(O+ zzGrTbz9+Xz-(#Dl?;-BrJ1>2ATqAuq|4{m_zfJnKbKb=7S>9`Gk-ie|WSY_!D@tD= zA$`+v=^IT+Uq9EYGt#$$`xmvO?>x$%eY5nP@gwOw^>*ny`Nz_C0@ugfEqzDaA$^DO z``|mJ?|=uSZ(r{3#l1Z!@3>z2tXswL)<$u>eziDW;rvoX9M2`i@w7)APfUp8kpXc$ zaHTlz=D7V*aqPHQ9M_&Fj&1)Xj>RvFqjR!2YMcwliz9WMI3mZ2!*`50CQlH@@X_K} z|3z`E;=G(^7jm4-^Rqr9j?+ISj<0<}94CED9LMh~j-%cyj?cKnai}4VgRJ7%U&OKZ zzs0d9$1?8q>?02Ou=Kw9ap`^S2)x?Xy39FpE^QqsF6Exq#`t*Z1^D$<+lNN-}3^oFjLUe9*v9p5UwgWIHc?GL1P z<&UKIk~^gL{M)4WoLi*#%pXecXxlmr2jrS4z(r z-;v+#o&s-XuMH-7YOv)JF_ zc>Q*@9ZLQ*3YA#r7J< zpWhVQ@81yHZ~q{+U;SQeKmVQBe)=1+{p7b|`|+>EcFSX8yWv5xU45_EHs36^xogDM zTohZmCAKU_yd<_@R&4H!*v8^w8wiVS&4k!49~9fgLt^`0zu3OR`I}y`eS_mGlVbZ% zpV*E~iS2U+^^*o=$Cc>CL8yzxI`{B?yGfBc>p z&z~X2Z_X0qmtPm-(G$dY@K`bK`MelEI$Vq&eo~CsvR8^^LEI^{<=7 z`p3t``og|qedb$YeR7Lff8Hn7pMG7eKRH#bKR#Zpw;Ug|=8f|EO3${hC-0-A}9seMhYO|4^(Sr2Y4OPOR@bN36ZmVzulL%fDU` z%j+K#%PS{~<)v?k<+-nl<>^zz^2AAEdF0DtdEoP6x%)7&+1ln{%5^dzICNo z{%f^ZzPdszU*h){R*B{Fmx$%l7l`E(XNl#bUl+@Vza*Buj}ptCwCOrrEcOFM{{4QD zf9@^vw>?Gv{BDuoJ4Aj9{(ABak;ncb^3V$+_dX?Z$D<-QKO}PfJtEt05ZSaKGTRhs z6hulfkxWD+HZ2ku7MZ?WWb}NI{^^vTTo|Wtrp4vdsDwalQ55;(8tX|H=j8dg=S(dhSYb zJv||=C$RgEVD}#w5!c<|fZNB#wZki}Yct~7HZQKlmbf}siK}+CxC%FlD|L&wBDadm z2L_nDS6stjfc1}wYt?VWwH&*D;h)5H?jOW;)^p-Io#Sg9Ct>rCe^p#Z{aIX};e06e z|DZR-^$|l{AKY79@7q^g@A{;;dXEy9~V+1?+zNL2*{O&Oaf}0dx$vC`jj|NKSG>eJ4u`;eO;XBD(6w>i}N$9#Chnj zI1j@9?;jNBK6!Dz7YwkhBhH?U;*R{A{H{qdyq4PyJ(rloIX zPWmpv{-2NiKj$XtI}_V}8pkQv`xC(d$K5M^M{<2QIN%V@2i_%p`+);K0M>X9&z(D@ z&vuPCb}owJAK3p_w}|6U4RO4X6~{9HaXg9re{4t`53Lr*y=%pB2loHwi^XyM1>)HL zEpco*MI5u({>GQYQNrG5u=_D=ec&i@Ok?XuvGx7ng4Nt#@p*Av#J%%4&gS}z!^Lsx zVd6L$3~<8!;yC7g;y7ZNI1cL($H6vnd}OCMKKQ0M-uEwY?1BAvfDNqsN$*?O|JOe$ zy{~*udS5zHdY{AoKYgt9K5?@2K60k?K7jqd`@7P6`<2qW1N(n%SbDc%`xmp)+bK$K z4f|ifMyIgXk!z&a$1%BCdWWx)-t|{Y@2cyhcRBX|LU6#j*m(46@9E%xuiYfQCt>f8 z$KD@xv-Ez3<4}%+u9x2Z!4La@0p8283=GgymtM(9&zso)*Eqk79eyz`J>3)~l$o;$$+w_?+8dd}fE^CIav4f}t}cctgVwbFAO^&eS~p2KIP=a8+^bKteov)_%C-XT3U?DozbV*dyB|J6Ii{wFZN3%7~=8SMCz*z?D(75hWj^Lx3zgY(VX#eO|@ zefy%=H}QKGtkAey>?PjG@J>blrh6vTc}RP5(r|IeNh`x)!R ze(L37KN=dyd{6(=JaGcop{j}Kk`ncHlI6&-xL<5re&4dIDz@sZ*z#?$CF^1fWBa{%u}vh!HUtJ(=Mmc#BVxOBRBRUvi0yx{=ikDv|5re4 zUmX+Mm)v6e0=ED2V1Q497e0aQ|0w4VbANA+J;4vIHDa@4YyXYy|L56a{OwXP{(ONL zzvue5XNvKw)5ZAtm&Euf_WvhGi}7P@@hzVa;|4Im)d!2Q`2aEI_7TqVY-{}SUXSBr6CT#PU5 zCC2AhiScQ$!l5UNanM6zd<1OrLGZv{<6`XbJ2CoB6T|qaSa*(z^`9RU>))bc{qs-6 z`l3y&&wf#?PbI|qixa~b<9Q!}`x>(nJM66fD#kzbiv0iwwSkI;2vu+gY=}(LG>vpkznYO+d z7VFWsi}kZ$g2Ub=)`Pz))&nYH-S=g&z8`Gy?$gBDw_dEqyjb3TS}gx~zgS*9Ml65A z_P=ndSf0TKKM5Xq>?>k<=s2<5`)RSQbzrRo9FMEjmp-<#@ z*#D>A6#2y~A`kyTWbiv8Yfl$h$#KcaBIh3~a?a;O&OA)yv;#y=0Rx;!yT>gPInpM}4%}Oo?T7vU zz^7!{d$9k`6J(j~WO42Ms<{5~O>w>YZE^kSB5}R2Qe4kq@1Ml}KgRJ8*Y^&H>khEM z&2DjB9}(B~S#fQG1DFK|G_e0Aj?C@iitP|r;7)N(!vT!mBd&h@|J9F*YsJ&zx(FM8 z-t*!*`vq~G0S9m@_W$IU#dQK0;25yM5u6W$2RIlUaKKJ+eP~Z{y&nwl?)}8ocZ9f% z6UF)VH^lh{KKNfR5a%DU|IcH8e**{b%eXin#RfkJ2XN08asKEgasKd5abEXRabES9 zI5%SZXTSh;&P6am`f+haUl6As|9|QYaWbvW{$6pe#{RF^OPm*dNSx;#B+j$J0B2zT zPd!1LCu9FlI9r^@oFmR7R*3Vk4dOhQ_7CugbKj&m_rm_~(GaHt4#0}te~aUFj#seZ zFJbGS!>&L51L=EWlk`2p{RiL$?gj_kj?LeJ?Y|Zru-%ETH;Mm03=gm#`@ahNzq}%S7lJR&1z((Xi}an2{r?)a{UpxEb3O`t{~2umq4@j< zalJp7U?1+km*>miBYLitKG`geH#dpnHSGV(Gvau$ERJVm;&^IW9KRS9$HVKyao;L& z+_^#=w=NgQjbMOl&Jo9!Z-`?a+u!1-oG6YQwm-oU;_!Sy9OKykLC$MAR)PsG!SY?fA-#V(T6$lA19%23@Z=Yz_pvji_aW^6z2A}EJ1&vln+K%#`f=&qo{-*6 z8R?znXkeR54e8Cm0mR?{0@(9u?Dr@_%g_X1Pw0S0h@FRa-9w~Es9I`;n+j+YYB^Bgw&Y5evl zu>FsK0UqG@-BIbe9h<%b4&Yks|2Ay=;+XVwu{Ps;r59{AE3|3fA zxmA>34pz8uwe*~Ox%8a1LV8Ze_J0lk|D+40=lF5yIST&aGZpDM6b|5^YourY>!oL( zTcqc`KbD?lV1S-mrANR3Z(_$^!;gO%TmB-p{8=!;Q`r1p;Qv3&`95s>o!IkRvFkVD z`(J}y-vSnxze(&ZIDiW8xe$kka^r9YgY#lv+Y zKW(MhPq{?wCte`--hLz z#>Rg!B(|d||Jj(>K8gSTF|ff$xc@=0!~5U}-UV*xT_H9LHveDei1GLH#rO+0_Ydca z@w;z|@oSF9PZ8s1$BS`47~n2^^xF;-<0kC?4-OGy>qo^{_@Ef=J;kW@iIK-=PyADi z5dOdCWiiIF|AW65W9?I7tbACEOYRlp{JX?B=Vmd^yjG0U9uecKSBP;E_Wt z;j>%CIP5>fIJhFle;gskhkhc)2jD000Sp(|!`>7_u)l9&=l_1WSpTv^tbf4fK6j{C zpI$50C+-vLqx*~X!LN(;o>Rp7qvOQ-LomQ~_~ci8R;(Mr1~Zhce@v{!{}5|>Z?Q(P z|9&vUBpkr-n_}JI5bKqfiuJPJi}j*!i4`1ZJ$s{A&v;m@r-Bc@a)?;}bDda^-7eP8 zfe8*LP;m%#A2=e`{T>tR-qgA0iDGqKB364=EdR!5e-mH+wNHuVW&HjZvA@rLO)O7g z`+sqwSROt>EcYEDmOHWkw|-14H|`^rYu+W6Ej?nH+bNdjt70ktQ7mx5miVJ$3I0SZ z?%TyOcCA0TT`ZTU#By;!EZ-Xu%Xe0b<(vIt`NkDu`3l$n2_85WF5q+D7t5!< zCzg+YQ!M{M+aIFc_kUI_@1|dUpA?JnVUf4@5_tnV{8#M%AF;*HWB;Ef_VUEvL>~F0 z$OF%c-2E$&+aDI$@qoy+KN8utU1V`aq|*?oWkm}3|EYjTWL(6zUSx8)$nd!$>%S?o z>c2#me^ms2Rn9$1ov=2KX}hPXw$6w4Sv(6-ZOt|U+9~K`O|Tjb!p$|gW0Y=>-Kr++^u_8yINk`)^T>n zt?g-l>4VwF?tE#zIwqYD?Q6Fk9rMz>ne(D$w2#cG`Fpp%ndP*+&i7LNW*@bTwxez9 zeVtb=!;f9p`g>{a%yL@4j!WOwceD@PYn@yD-5rbm);?=_ZC|gotk$_yuUU5Yw!6>t zp7vv@j&56Kzk0aet#@hd%rWb{>Y;PR3ufIqKAmH&PwVQ+L3ga$p5E`aqwg)Pk6Dk7 zN7s63j?DSj`n4W?re&Ab#Qd%6u+*M8=B07$s!Qi$sh?&axz@JLd#}AyXZOB2Z~B~y z&GBlx-M(u3ItJ##?3-S9$E4-8-R?b|FD!5uy-(4EZuID;deOJe){aq@* zt4{5MzO&Q^b4|N-bnDkWvoua~KWTefmmWF}>Nl@-d`s<^?Q0*q<5=p0j?rvepRrzM z-0#yySLT7?v~eg^N;wA{h*Gmd9CUgdac>3Ej&Z#eK%yKR_#(mtrnpzqD!I#;^(+FyO9 zhhFPAwSW5l6C96l;P;ws=$>Z3nb$9J*5l6{Dz|^-&~nD?W5Myty}MDnWgu4$*|tjXL`R|$L_zmw@d$Zb))JO z)=byXlvU$q{?`yyGpbutWbnW%NmQmTz_BnRF-z~2;KfBty{}z zd98P;-CcERnceSkZ9dbo-FIonytmX>^IG50wsh`v&6du)>jLw+_C@PbY@_|8ysXTcUHzmp|7+aZhOV!+sqZcQ-qp6=*FI>UDQNaf>(%S-Z+%bur1f;y zv^(eBdUA{ZXZ;}bR0`{m}h-o%QC;a*0VciohyAtK~r8ho4>p5 zvu5U)sngVrI(D5mor|SCYd+WhYhAjAOYNECT3QpcA6hs4+I3(1tb1L@#shPny8A%u z(lK=V-W{vfp@-h@o^@Q^wbOE1pZ25sd}-|6ahdm*+U#DNW7m6Hc4^G!HNVZeyY1?^ z+lH3!_MAM`e z#@(HFp6?pB)~UaD@8fR2&3Bjj(47Z!-n#wPXF9i!bL`-_f#VsD=Qw`J@oNsX`)cd; z+`XswALh7&L!Uj(q54+szuMwwmkw<1?sI9%m9C@qQOBeG&};2Sx1Zgy@Ozj2f0XO4 z{nvIBYv>t2*tGv$oBnrxYaf2Y@dC&0SV8-x`cB``a@r4VL(lZX)Q{SSrS<$@@}>8+ zpL%rHs{31?>7nD%L+|UEa^1T1x30VSOxr>}%y+acJ(k`z%PiGrzN_oIBsb<+%k4Hl z-THQ)WAolp8MBUV`K30@_qn(0SyvXi<+^L4zxAMfQ&*}DRUNscPt1E=Ij4|mW6`I( z`l9{THuZfir#hu;f3+Or)ION~ppe;beMjqBs@L2rx_5SaZr;=Pm*&EJU;C`{s`Y6b zdaZM;?`iu>y2gB0_ou$Ubl-emue*9l&)s*mkGgL9yxV8a-F}(<)Hzvd*Bqlh*WcQX zj#mj2djJ?pX5 zK9}!|L4SA0qhnkuV?OU*>v^~G=3HpmZuxFG{jFos-%E!%*6ui$%I_LCeKyl8wbv`CKNQh$mvf@&5h|LlaYOk3SSmB!WTT__~WP_}=-eM%S)dKjMu>V!l8m zk$!w+8s>O5VQmtC6=6Q8!e99k<#uM>qIGPN3eSu&w6o`al$yg+nNThS6bhcQ` zW^&n7I+rVyip4^<^mw_D%V#q2STq=jXL7krHeW22%f(bAo6A+2xnwGxE#!0AY$lUT z#*^txp;ReVXt9w?q>H(1nwm5DLN>>Hg;J?ltJmtKLOyLvWn zx>j#@W)?Tj&&|%yG-o@FbcD*nu|y&g2?aXANYuT4-G;HrNT!-i#bT*!HkAzeJf29@ ztNz22{Mlqer6sb3rNF~BkLxcT86Vu)xwWZRr zSj0bi<>eP&v}$Di+Tp1%Gwu(?lDT3slFn6Y&1x)DDpqUtdLf;TO)%mCq$|U0HLa9*5 zWJ?vsmdqBbrDB;0U@BOvTCGuU&CYdNopz&MZ`Ey;K&`?o?eT`j<>)LZN^s6pzMoEJLAA1@#vHwA(YY^NaJdO$4x9Ef(wTdKQsJ@Wa7$jT_Z! zdvW8YZC7pHxN+0M%=~PN$Dv3pl|kg$d9j#h!}`G~PomH)=8}m&X7c$=IvVl^63I{? z5Kimp^N1yL!Tc~JwuC7H?1w4?sUw64TA`2%Mq=@3B3&rZ4N6uDY5q<4J%MDl%$Qk) zO0(T+(9fE#NU>-sS&H&dtF5nBO1UiCEE*VFzjpQd;ju|iAQX+qL*bxjXyv6Bo`3n! z>Qx)Yg7HYy7eq`8iAX9}skQ5gT%}yEHyQ|k+7k@LjqE}$iwNXmUVk`BhmtA+rFx~p z&SmG7a;gT9*pjW1*Pm?1B6~BQ$fHb(bla$1Q%T42g?w$Ml+ES}berYPr!wYS)n>if zZZukjbY7P_mr3PHEUl4eS&F49tuReFTXtJAfyQ9wa@wRSfl84{(tVIq0jSn0>`W@F zS5=aYQjiU$TF91*sO^&0cv zo|#{mpJ`MP3UpAlSuC=sXH6mqAsc15g6x3UxE&dNq$+ zCw##Og37k4H7Z6qUdpH071>BAmM=ApdZbaSHahe3Gwqhq^waBV*-}OTcINZRNGyTY zW!;$YRK)KKrt*0tll>bG1ice$R$aNl9r8_22a>sTI+3eWb1I3(%$7>|YAMg2WqnYZ zNNGM33Is!}e6fPCX44_>^z_u|;P}}1=;-)_ClqD+SgLAszEekjTj^R!b#*ivK<&g+ zjLRrCa`{>g-H|P1lId)zTtr{xSs4-c-ta{cJEFW~q4*lma*ip(fqSFKkH$UTyl&BVh=#({=QvR0)E zn**JZMSV3pGc)aWr(LhsSj{Y&YKK~#Y&ug6kB^MIeW4^XSe|Ls3h87#mdv8NvS^=Z zD3K}V)0uR!!uD3vgB~=RKdzUnwGR8gQYn`TSl4VemPn>C(J5prnM@~wz5vR6a(HlX zB9c!hvxzv{C>@R@l33>yHUTSONlL8DBIMYCXa-d7YsF+Z7)qBaXigR@8H>jP!~N?v zOa}r!UpQ4nZ{?dzSU#(YhAmgC6smPB5vmKd&}dXmi=9bVFb>T|Efb4GP-zH;$K&&c zQmT*(rD}6_%L4icEmCGFjr68iAQ;Q2YHc*?rBbtq02FwXFCwiC3@kdoV$Np8Xau>% z?~<*26`HWtnxA1ec4p?9t+~$p;tbntX0Fxgw3^jw2~lCX>1%;KQ!>gv@IuL`ZN_Av zyl30>`2`eXt6A&Jv}R}Or7G)PE|s#mauM|eE~)TRx*8ZC7#tdz^0}uchK8s7F(Z8! z%f)&je5!_9on{SLEn&S%nM9Vps+1P_T>)=k1FOx>?CiqEjSKUb0dywjjuIuK@HWz2 zti~qCru-4?E8+-LDCN?Cr+m4Hw5H>+Fq*D}AqRSt(OFeYYNy?5sR+!@&7l+P$TI3J zn^YJRVd2zl7Pv$SC;by+6aH+yQmhuUMK){}aW7WbA@s^r15cp3&@1Rg9W**R?3wn) z&?<>IXet(thQ0l3SFaiOxToB~R4#>iY@q9M8FO1!i`8J={05(F-bBSmop3OzQZf~U2saNXdA_l2bt#>-r24ip3E0&@q zC%!ClV- zk%>)B2ZGav@3twFdN3YEu}9n!{p;7Q8yp-D2K)j4)D+qibqN+u#lqQoYoSvGGXx?) zqd3|F9h}XvGp@pF=P>jLM>fy2FuhC`17rIrqRC~ki^*stOab<4!6-dnME^#DQN%W{ zC=*ML_C>XUJab^CsscvPe-Xrj#>^m@et$3=jfNwUpr6hu_KinaVnB0|$x(NN!kButdAXd+mciz6bU-SRM2%rbP@nlq zs)V%y{#6?d)dTgm(Y&Qmqdo?wjucg+ip&|%HB81sQNIryzv_xrgPw3G;GGy=bH$pq zt5>aFH#*@9g;UnFxQj-?$X;s#Cqzg6B0Mn6A1VVQz)UizRI1WM1gnTrsg8bSQUGyA znT05Gq0qjN$)~)&aL8@=ubIZ73?@_`MZKQU;gR9-v8j+sfLqNya}kTD%ehpkJ-d0K zUCA)zw#18=Yj$*{#sI6RJCq9=99+|CD8@(AHJbIA88mhS9mGJ9(Rdykf?LF6c0nAQ z{|?S87w{aIN%S)+oxOs2K`+-qL*=Sb-bvM{%#?bO2rr|=)5T@g#SF!9BG|)?x!LyI z#x0u{X6IV9ayp6t3=M1;7#tWGpPa@cqA9jSsa^!g04-4dDRzQx9vol*RFo2C8|6$M zEDK64R@$?81GBB2I+nU>aqt`i$fWX>q<7NCT$?Bf>xoNKWd*>01+}gWv51~e6>F86 z>i>4L)4{9gG!sF^RvY z?nt_1t%&a(|LNK+kPCGQVgjD1*N*#{!}nn*DDp(2PzB!AI+!~|piu*#!c7 zn&enX6@n5Ln`&{^0M8&$MVD26skvw0l#KlI88%}K6bOF9WW`Ye*zd+H*kEz;jOXgFL8K&>GcmGRoUjApk+prbuTpl8ndw?&yrxav@X580nb;&IDRA?DI|sLLhS%8S%m+NR>f28RyzAOm0uhKxj6rxou8)8h5O9&r;r4>y)Ja>$hmB3KD^ zg!3N{8R2v!8joRsV*Y3*=7%ea8nI3^45|k)rRYCnEZ0C1xLi;Rxb*D*M$>5Dr9ftO z1|86D;rF0q&;hEM*{Ee~J!0Fe7W|>$#Hx!fxcJ=ft{Ry3O-~N3U%C9k@16UdbH97x z@`0&n!jiD0D&h5% zYw$BjU$u(M$-1ONN@wB5GJU$wQI*g-8B}`<{aZ3}QGAzbrCHA=!eJx&*L1#$8&@x; z5!ixRSOHsAc?;0hY_p!n`^aPZ+Ox<&nH}AkS%6lVYceUALyS_D9gQ8t6OH(BVWEzw z33ZYPg>Y#3VdAYyy)y$~tQzgd8mN{+B9=_XLLN_m@nNoGYK`M@Sce3pW-JDp#uwus z2%ctjZca6JG8*zsj1G=YOrk|6JrEvAlyp%Y-<*-ZsZd0MqChDu5!w_Rg%kt=NTVwE zY#I*}R)+oEo?-3UEeslp6ewX!$1|Byy`0QdvKY36k=lXEriVMEKVImw0hdmSh zt5;ll@kN&|U%6%+onR?gDsrnz4!aZQ1B?twf`?hk0lINO^9h}th|lW|Mlj#%?CAc? z`RUv0UKTXiRyP9D5tB5DEFcKHx3Y zvih*t5Jnx_Ol;TcgS;K zJ1|fSGXNFc{|b@tj@bzKuIe&jX%<-iW+j?RhvD-szu>}kqrqq>I6c6|5RDajM56e=+s0u5t}v4aJs9`Y9Jh5QxO&;oB2V8b1!)k$Vg<aW*Az&UnL12=C0vxNd`ns&EQP_!?Lc@0eKrL{78ON(qFCkgH1^oj}r6rYjMlzu6 zfX4Jzacw?d!4TAP`EY1z%&RJTVsv~Gz)DZDC>o#)(~agTqEeYhHKh4M5M>{YrDGwa zCFGsVjgGkkcpMl^D4Gn$0T0~B%mL_1IgB;zCW8PQ0pQGzBop`-$ppnRSp&kk!hn@3 zOC)1>5JBIRKOCH%9vvJWUp+A8p7wgCp#aB1nYQTwI)EwBQy<0T4H?CNAYX zo+X}d3kqa*UY%o%M5Te+*U*Z3_322&H$64HVWfZgT91Ep2xl{b*W(!<>|eKHtuYW7 zo%G`3Swa?%#EOM9?s_;Fzy~!-*Px_laMjr?Nq}iBhLBVI@aM;R=r_@JNo1-=x zm;j<==*?EWit*RB>ZIxsSgHyIB2lR4faJQPhSi?ngm=B?YdZ(W2$t(Ac#cz=ynXK~}g z;-*a-H(j-9VPW&)rmbxHIaral`YlEox{HlbNM~Z9;NV%{U`nYorOQ+TxMV^yMk;^+prnjU1W?;*syV}?zm&-k zRnkBUEalw7rcDd;vsjOhJYgs(OKBMWy`|c!s0w`K{coP6-}v%5IDfjRVl(( zV=N_{W;7&(8vG>=LIYW8Rch%}z#U>224J~hqBLTtgoznoVtH@`lzLM~1Yrte0XOA{ zZbn0dJUx@RH7Eps7(IZQDWN{h-JtS|^Ofb>eOwQ|K#x0$mQ!j-r)gngv}feP7U|3U9q-* z!}=kZ`b-Wq7#8eu0v{c88Vm$YZrqj*#Gq+nNh2LlTM$ngF%SPlpNkN^U`OM5zypHr zp>d(E0@D-2qrZYKaL2qC{ zHfgyAnkZEl>O~YW7KKPHbWNestaRq70QU)V0?*65E7O$L{pN$C03VN!53gIZ;?mWV zI{OTieVf0QeL$-~B4i1TRVV-$Gc+1PTSh{`C>ESx#KsQPZAOF8#d;M_j2Gai@Zc1G zsEe+k1fGufP`PMPNR*r>4+Y~VloE*;(Tx#YDyBK+pB`I#-kGPLd+GXdH=ZQ4BYgT~ z{|H7piB#4aM%xdMS7+~e@-T|`0})yTLgsPMTVPM-t!gSA+#^&>UcqdV07i`Ef(r6X zdKfX`LjW)YzsDO4`vX%GZn*wz0ZkAV_6dO>R1;ztHbH>G5bD7)8%4K@Hc>n*twz~X zD1Xoul3Z^q&eAw*qk@|i3G!fbG_nXVEY`CtXx!skOx!CD?rC6yEd%x+Ddb7%OdC*B!bPv3=@Mxd7{a* zi$*=qDDaa3KUmRdRIvtn)hIYy&)nK zZDKIxY}PwHHaa;rHaYDM_|i4NKJgiixi|4K)w_WL2IBkT#rE9XJZg^Nv#c#ZLzRiZ z2!R-?kmu^9;B1%i9ta<`7Mqovt*`^>U`S@{aB;InjAq{v0MI3F3pzgq*A-;j2ffpH zctZng*ADq$chmT1*XM{Hv|6264MM3!DAsBk!~?g4!b58gsr%iZDaOECauN}!85B8HX>~x z9)y}rXF&Sy2^f2SknnLb3fl1aLXl92al1XCe3@L4xcHMWD8%=@SS~wi75E2p@AU)$ zr`(SQ(Md798zZz63GxNPm<^;elTDBznHJpZTfL~i^&UG{#gO7pc z0Gbq2snFPDzzEKH0`N#-;sImpApAY>WjI9zmaK@uRnt*P#bZWd3kEWrr~(JTQ@ox? zz(XoMo{h(0-|;u_M}QxdN)afKS0m3BYLFy^Q$RE}77as?W)Kgfu!)FOu23)XJCRC} zZd9xBF3>oam>66;I5{;5@2pWMx8I08g(rrnm05MPJN_w&2)fl!1kj1)QYxJw+QK}c z)?w~^K_BcB4je2Dg2-gx5+QDdT)mLWV#qW?h_Xic5V14D0ptVUnJ{r);{#a_(e6S> z=M8la&9M`*9h*m181jHm0QZFs(5NBHk-%0c46HXwq&7tSWBn^Hz4VgHF1z636>A3Z znf;-#XDXa1;P$f=`N~YY7?~U#7~HUaVALDQRg|%)5jzY_4yX|1@x584Ob%8OyQhZq zcf>?l9pWyz0)P%mfmLMgpde9j@vw%PHDW?26*bI!kgkAHP~C-|!Hi>?6whIhi`AJ; z3s`9rbt;RTux0EtZFlXVIJ2bEfFw+5BjAi@aVz|zpZx~)h?2F4L{59f(S!Bw|ZEOop>1dCh*RbzP@ zTGdSHilGEZ>wsfR8ma4%0>T~aJq{=XP?g4Iam?{vJfmwayKMR8mw*4#E7lGZK7sX} z9P>p;OfWJplItr>Z8uMEg zXCTcgwo(L{Rp&B3-(nZuqcxVw^6O5^mzWWSBZR;gHn!^(qwr7}Er+cH_ZQ-D$s}Pw zBtWwyVoHgm({)7y2qexrL2n4UT0WKz$8abSU~+FVSwhLWibz7KNSL@P*auJ|Y>py* zFb}H6M${^$@EA=5O94cT7$Kq_wCIEWpZ2iTGLG;PE*BdR0==Wj0eS8Lxl<<9 zgd);pD-!7P8BKv>I3c_{HXNetdk zXfB+_0{%!cLxSW^0GFoAs8?C;%+Ig^ksOwQ@FRMOPVxq-TXUVj2gJV7>*!A6b#W3) z0nBj6nFKkriG;T1_mW8J4n*U?a}+2W0zsxrw2D-#Ry8AHS*52SoNh-)qx;Qi1KXlE zBKX62SftyKnlL-puEUkbqtII9i5BbmAcWKu77yYEM<6+kj1F z%drcYbIK8SgE~*fjijGBC8*(f#p@5UY%wCCgfu|*0|O(Yqhrvflgw#^FpFoyhKT_7 z1~{ow1y?^gIy^K01x7k@78(bS6~4{u@%W?Z6qq;7e5BK5U@{vCm899TniGD=mz%7yRN-&I=kv|LsP1(@+9{an#!ao z>TE$=lQG&^oQKKBYr&zwHj&AAD;x-l1*xeKC27((O;N3tG^aTOqC%8m0ZZ9Pg#D=w zM7W7OEtQ!F94Qv=3Ro;xf=U%sd+{ZRH<38-zNMHg1 z875fm2}TM4_cHkpVFbYK-AObIY0S`4nlQK~yFvl6#Wees#ULqB12v$;q;4hbKpHm% z^`?Xj%fzB9p$Ghd+(pNdNzmDdxkLh>T~g^1tfIz;tAGV;DU*n*))5g!B*xP;^H#!S zwP;!%920mH9m7OZ0ik~*`yfnz&>Li-*~Ksc^XSMNaf8_oi-?}qC<$C9Rv*fz1PL(* z^MeH9QLtrs3;1eoFZNZXVKi^3lrdDm0cFF9zo@?eKEZ|ek!NY9<^-Y3@Cu0Nij_yIpaD^hk7(|rC^^v9N;XjnP5qX$m3Y0Jzpj*Om z=pN0oM2eC5sGkIO&A-JL!-MCf3G5Jngt#>ZkkLFU8pX=Rg8qPKDiSrqcZPzYaHdSo z2~;J41kD)G=lB~#z?>tlNSp;plSMGe!77ljixuyHOfar<5qwqWQmf% zN<6D(ZYWB!tdb;1qGg%!B%0AEMiDja7O1;eHVQWu z(^x*@XiO8g+3nzR^rk{#Tnjvte6<5;W)?ImrP-D2X4HrLy z$G=7rhae(5k=Il-=yQA1m1;7Ug7eVeG~6(45c!Ca2hT*K4D+#A9#U9S4#MGRTGLO_ zF*;MQOhn&({$P^8CZ^5rnVy{R2Pcr`Y?Se`-jgF^lfI}Q>42Y}3I;vX_Ge>fmEKMm3lHtO{ zXg&?y!n$*f15MsC1cQG2z%}pz#V8L;{HWj!4KH2R%}%tt|Zy30rCs54o2Z- zJP5p8d_THMCm=@}8a&7MMY)uSuyi_eg9wE6;}wnK#p6X1)gj41i1D0?0Fe&A*X#2D z|Gbls4P@~FT@nHJMY9&-oX6XW9(6VR0*ubad&HXmD>bta>Z zNk`d3_F%&m6J!{oh8iINR05!JV9;%;Yc;6DI$#TEOC#EbOz5WDpA6R#2tbV#J=a|>*Q;+NM41(VcB7JUc@eQe^#?u;d@JIcv`|v z5#N*#sv5mj`wzZHBOhcj#u3bAYhSlPG1Ly;$SOHVqh&>2qVyluV;!fk^vaK{v1{VTU z&8K8JzDgiThoqoX+>fpxh%huhIWghZBz)Wvur=m`Sw*+wcN&?n%8|yR6GrS_HAvJ4 zcMZ`6rmpCy)B@!Q3kpXL(MuhN3 zI|JDBxe00j2k;9pz)0 zf8bW+7>3YmitqZ6Z;Z|%OOx`9lxo-}K89hMc|93CMY$ieZjoz>Xr7Q^+4q@d%Y5FKhb zNgg0{0VnGV82(v4$^vsn$c)00YW}MkWmklwC<;9X0<|SdKot}+8x`{%CozLD5E8@^ zkca1~m5tmSn-g5l@@s}&mWMb8cr`du3|#|$x^Ik9g4h-r_UvDEt#U@|#} zip5MuF>#^b)Fl2QG6xwK@|r4+ue{fR}E{8K4wKiX3e= zJD(lp!c$)GEHsVLymq!h#5q9rj4Js;4zryhcS5sEa*!Q}C7BA1nc2lHS8d&X)wV5~ zWZp6=Bps5wgqM^@M+1cUs_<$efM_60b(zdba(dzj9UvRyfP19KJ&hJor374tAgtC1 zSb+VQ9dr4W=0^H7ZsI&K4ts6M<0=c+vfysc|pd44@L*0qUoY>eH`qSa9)`La znU2NSt*S=UB=B`4-;@G__zDJ(1G(V=?kMg%p783r6o>FDLMhr|XAB(6L3sf7m;+Vf~tQ8#Y{a@#UAVT(e=rFyTdbd3C}ykil|6 zw&Fli1yX_*gRD!1z1TOHhAdPXjgq~BO;9Dk3n)#PCMChi)&UD41_sio^aI8`%~we3 zml0BZ6dkM8f-+SF@H`RbQ?@CByCDK;_`Z+<>aoVy^F-T9f;}n zG=u4#l@P55l|Rr_4wex1M|}@$3QG@U<qAFkbW>ekSVEHkPEkaY;ttm z3oWgh69nZA5TD>HExMwe(r-|s8MXR~tW(LNI+6Iat%iCBr5JXZBw|hA%M!g01(8N{xp#JdC%H>zCx^f*69uMCogGDqLsce}pKM_Fv27T9cvZ3j067|QhtDD^ z72BfAkLJgy1VNAdKbp$dI+{jGeW(L6soUfs@nuYnNL>gpws9U}#(*(KCBn&@TLAJ7 zd`l__TZdpSTf)dafxZNY5@=5AcOHDf4X7n#GzoW+!sUtwr$;tSO#2h0odGv0)pu4O zQ2;UMqU%SWns25^;GHC8V7dD3Xx&$dgp#cRasd+Q4oB0|8aBq`!;~uN1Tzh(&j;YR z+ti@(5zVjS+t})i>Tgxeg*sVL_!!NZ4#opNo$ZXmR5^#ZA~T##2b-^E6co6P@B9%% zGzR(zOiL!w=B?YeZ(P{2Y4gGi@ooa2I1N-xh@n^j1?E)*@O2WV2()KN$K{5 zDS#KDQuXg~*iln#Yn=qH!&mM2>+F@CKEC!fZzYf#ks`YE{n)PMbV$mXMDr zf-1jArcwDT`p9Je*rHJ88fC>lf&OO0!~xCTAkGH7E`$cLSiTg5BBcrzo2+@9faYAA zJ+7!PU&E_}B5ri_jtT)*Bnt{4A_2tZs|HXaK*7jI)UyUQ;r$wwInA}IH`w?aw`|)s z54K$7qe+AS>A{?+j$$8>JDQVgNS;F3tsj^GxdSVJt(pbt8CkRZ`Qd`%p|j zz0<->J&HrEAX%A6Y#e{j2PGa$>mLEgXZT)_7@7(eHp&*egPo=f4?LuM94Hba#er`z zAsePOi$6n3fzO|*RNJ`pOcz#6K_GgK05l{?9>io6AAXHyNAvPuv>Oeo(hqAzg>PJvUqKzzqncdQu%#$csB@Cvk0@Vn+fO$8N>J2s7F;H`W+?_v^2>E zQQsJu0$vO|e+-3O8Z>F*!34WDolWMl5fDHo8;oN8_)-;s4zdGgvpoTbMgHoA8Mje= z#N4SDgRhZJQ&^P_wknPmFhT@C_&yvmh>vkb2CiJOVy$f?J~liZ^zu=n*AIDWOk6kZ z_fJnuczm#tWHG4PDdxhFc!jT!$$ZC4l77N7D34^vio-GC=PJ>i`gBV&@>$*>Xkg5Zrp9P6Vr6d@%VHqs@WZ_Q`Y&zJ*2yo_3h zJPjOnBNKvG1Fit^N`E)iZz9f5RRVErCtHaq0)fm{r`93~mTV}9Rpoh^Gd5K^%{NNy z2po@qZ_*P5y^~#p1F0gA%-{!-k`>9&bPx_`w zM1>@oo`fQq^!bhHCs0Bl=O`NnDmz1T3Ti;qtQlSy@U z@%$(CuLNb|0sgQJ#2p}&0Vs{<_ISPV|BtCVagr>(uEXBVS401AmL)C#i2*p6y{CJ6 z@7njQ%&N@FtbNJKTC(FVhjfB;BB5GB*3KvSe>6M|?`ju0)Gbcl;(Iw(7Ab@+E) z%d&$I1Pr>XGT*!3z2}~L?)eD9l+KbSt}kcEJl^j=Nm5gN)wJCMl<$s&U#pmOVYgSZi`Ri9h; zyvfTVCCdt8q@-F(gKo*1Urtc>O8ahC2)qFgyE|)@YBOXxurfDmcri=F)TsH~baM6V zPJ8~LFlSfcPx!}ivTjVDA=}!x_EtTUBa#+Hiy&jw0AAyqEv^i^leAzx71%VD(gIFB z-@`t$s*H+b1 zFc)0Yxh^gf>8uh~nuD#Fovarf18aaa&8FO<**-DuR-sb`eYY?tY1g;sqkH#6Qzd$~ zD9JZgr>8B7C+TU>Of9Ucw!uNFwT60{g(l;y2=FotYt38zWiS%5J8&(%yxi(A8xZ=o zju${kimE{2q!Vv4FuE;F86qtoWdEd8r7o>?>9R~Lj_CUNSZJHUh%J)KbZy>=A1Oxm z?w;MfxT7wrTB~n^z#IXx@Z~Lf651yZ%og>3OnMqx78Go(WJNE6%>)4vXdt*xm&VM} zhztX29I^+}3?Wa$*oBFC!}_f0;n47)h5hQ}@Pvib z&Qejiu2v>Z6+kIR5gpoWjP~{?4?x29Q!zoi7nc1@=V)mb_6*2z@HPwfwpAZBN?zDl zEmq#-1bLv=`@6A0F;6E^b)v(nP!3R`*c4Fv0aYPDUWLHdwoqH5T1(5DaL-$boUGd# z`Wpt>;`Bbuo}zijyyict6Y1sB;N;L6y|mTNfVHrjdMvNSw z^fnPRTGg?!@u~ke2p|~0e|5vSA!|MQSz|^&UWe7wKKE7wN#y^qXz6gmI(82tOhz|t z(e{#L7w4;4EVGeDLvTWG4ki_Qv;L7)%!9k$bi&Z9It!9U@qpEV z$h)P|+{|HaxmB%YIBQO;nx(`PG#1LGf*GXYq0ET1&X@El;2`VAb!t&Po{M3}6V$fvHKhKgTp{V>t z0*jVw>0X*0vx^0#|L%&m@Ul@I8+!3v3~+Xc9Mkwg3~cNugITRiS0r05&i5?-2=&{x z(=Yy_L4-RMcvMSgGxC3j0YNL_95`*_=21kIAfbD>Zc);LeGewK|P|ZV00X0YGXYJ-xG5u!7bTpXu~()Xal5J`k|A9tE9oT)2($(chYv0~ zHIu*?Zo@qG;B-5i_&AXz;6tLsQDQ0Ms}f8An0iH7^N`6?KDLo0_3GHuho8Lv&9A@v zEAM~s(MOLI{mN9;aZDbl{J>@>E=B=Z7A`;)KPtb}`)$Ms0war1kbbj|zSV|HyeD#r z0}Rm{GD8{z&6+4!U8v41;t;kG=;s$9%D^tPlWuyR84u`UI)={Tu_QyMn@3HyHx=7x zd@zla!2uKD{E5hq^;RdxYt0&6iNmXd4Gj_c*>ge8BGLWU%~dNsNJ3kPz-2b+ipCo} zv1CwyXfy z4LEAr9rL4QpqV_*3Cr9z9v61cn~nK}Vy+5${5mB`9jvud6;Z^>vSwS_C#VO2GS7@4 zm!7P9aN#?dWuqNR)zIov&&nxO*Iu5ToSIu@3UF-h@0Wp+jo4`a7);jHH<`)CMGe6Sdf}PHa9kT5OftTCMQgHL z1Dl&BcCBSoktDU4{SFK@HJt&M#x-xX)EO-_2F+atM@eWZ^he}E6w-*SyhGGA{^Tz= z*C+|a^{&Rr`2sKktoP4j8IjBY(g!iFS_8JQ4+P}Dve=lfPgRJKY6kmLl?*#q`S2{v zF{x7c*4ZiQAHyK{J;{sSOMAmA(dM0hyI4-gQuL*FMKn!ZYH3M%Xo{Ezf4(Ij<| zy%bei#?hvST1pNDGa2(omg6PMed+Tyw|aK}+AH&9WSU!y$w9`$rDVvN^EN#d1|r6D z@^k2C*ZBq)H;pcJ>FjZcM;Ee7CJq-{uMacX38smG^bNqXh zPZ3$R7b)Ei)8Tj@2y98-BTV)+xTO%8Tilef*H&U05iLL#QZi@t9rQAQWvUgHxPv8b zP9i28QLJ4#PqyEgKU+pgAcW;IQ6(#`(yS3%DB%V_OoFfxQ^XPQ`EG#U7Z}Y^FKClNEf>C>a{l%LOnAwP3r=L z^iV+)W;$pUQb=0_OjI2BtRM=(!6-j0uXtzpB6JYd%-0T$nM+G;c<64 zoC&}pzgZf>oB(A!YD&V!zEeg4Wdv9o6L#$d5%BcI(0FxXVp@6Y$n{>ifm>I7%-fV* zJY#BVE%(*2&T~7ZTxqi`cWT;an5Rq3iL(&SnS@|jS}1m!RO=|Tc^)+{DFwqwQCENu zZ2$0Khzy;JjH;^PuEaogbX97?v7I~ZNDj={Im(!bmV_f+VeFB-M&AoBh}&3_AB-`_ zL#KKTJ)M?_$W(6~#^m>ug(LV}2`X0qLpc>*#Fb?n5W&EVofktf6C%12bcil16fF#` zM4R~w7t2ARlTe&0m048mz=Y`ri4{)+#mno-?0`4h;yMVzjET#$mVy?1-5RBlo(}>N zq*j!`zu5V!46CHEX#FWiH`u|}YvaQM{k;QYwOP)nAfAH;$}L;D9v;O*1O~Qn`hnil z)yYY#0_q-TCE`KfQdZkJLS*k5-{}lD`8PtExJ#4oqWY7G$>ILNVI_sxr@ko5Dd7bc z$bwmY8Pl60q~om5&sD}M89B?cPEbTPcJr$IzYLhwK~z!}7GRSN#N({~+g3ga@+uEg z`8L8F^CjQY%=d@3f1K=<*2L)4(7<@CJ!cx#Y96z&_Ri5`YRO@Ci{wo^v!IH6Z?%nt zxWj1VSA8^z6z}e|iHsm8eV=m;?X~qg5}UG-1*NSvI!nNS0mbfLr9MeA@{3QUMv4eg z=2t40%8j2XXdsZTSrCZ;BbSWMnsfD8dKsrn3^}=~2VTF70jWPfLvD~Ga6hsYf~m7O zFJ-^eeS?tG6r->TqcOKkSJNMg^D7Sfweurd7m=qAAF;XFWb@ z@sYu${scBPgV0>JrUMXC3xH1c;~*EFtyV+C#C{Tu{U5_Diu&y%i>lm_&iVWt?K0G+ z(uOpbxdqp!E7ge+FfUv0rDT&D#WIp55vfeSm2an0jqUJqods(ICHvb)kp!-7NLIXG zB6eaf7Tr4;kBMN;tjE3oYgX9!6UAu3Qi04w&xn;*nIMT>Pouq9tw5PpK|@T}ZDek2 z#=XU>(g%Zopg7k(N))DnOpE+Ky>1x-kt2KS9#AR)9hUkT)FVvvH|SX8g&0UhY%j-t z9R@+Y@7DHg&`u*;iq7x&qk#^%3QsHogm@3wQB!Gkzes+Aaaz;hJadWpEaQezfj2zY zjZBu42G)wDA8DlWMgx35l(C0xUs{DYMy+`3#Bgt>v&PH!4C|*C5ev%-k@XtOc?+;u z83R#RHCawq8v5=)!IR^dEt&jGH>4cTOrOJuA&exK;w@cB7^uB{djSdo??lPp12%`( z43-`Ft{u07jpZ+AYI9Q)JpWYhIjU~N7yi>%&kEZ9(OD^T))SVBFh_z(*4s3&*NX5U zo(LkN98?GE#U6|<#q`2!^MW>dRs{)3e-3h#N2J#RRaO98rroJv~Ij{<&>mjD_tu&{m2voL-5S(}!bhfDeHQAC; zRcvKoT78x8gAOY-kF)S-RtZr9I-JxM8YaEJ>G&3fQ6n9P))r(!v9M~GV5cO&=gd>8 zm64Ir@$u2oO2fPf%VWSG)%vJI8mu>%&=oP}DW(mP@iYiw{lwt$8MLZ8$QE1^f3 z85FH=~G(gMuP6pePhS)Yt+!&JkXR$0aBj6n{= zRsi3?@2QId1^&v0YHv?U?m7HJQ_+(w z>arC!XGe|)XsK_cxeLTLC18z2kQZvX#=mB_%l>etznQkBE|SX$)oX6j%e^{hiGnR%$v#K>u4?rGab7IM+6$h&dT}6mLtV&$t{RYcX#m1j>co1dN zEDKVI*T&q;FfcGxo0xv94D@ECA`)ax*(#Z}m1>mtNx1&QtE-da3j!iJO~(>}m0cHy zWo{8y5j}TFcX{)Kt&m7&^L$Y`> zj#_!#QYZ8~kybc_@e^3!L!Ik2QryAzwDbO>GgF3*e3UKxhz3gCNQPJz+2s_k+mVI_ zHdYoKyGZz8WtGwTzcABB1U{s!m9JmuDNG4gwvzF8k*XN1{R^eqpWvP*&hol#D3+f^ zu<{-9dVLuEvW8hJWr-4*c4VZV-r06BS8zXebq-6rO4KIr#OkrtD5lN~F&1@8ei>F# zHP0(tgg)WfX06UIc!8#*SpIEPhhjx)h;g^976!h(b9ixeW_dgUo+VG4ki}OtZZ=^Y zqdm5m#jS^W+Fy+y^$wld_treOv_?3>O1kI*} zYzm0tc=Mopc!?Q0KTBMPeG>sZPkPo4tdbF(ruYUyR_E$?3`$)_$}_b}Qx|@iUeY^~ z4$rwJJZv6)3k`;JeL6w zzMXjx#6Wc&1mSulOqSNp z&QSt4*O!`Y4VCE-@sZtyMfN+PPxpW9ALfty$V!f9b}C8`I}BqI0U(W$yAjrWu>vVk{_^Y6!b`8b{}(iF@Jg zk7Uu)^P?jVkU#vz+2JOpj|9TOlh5I$g%}EZ?>N^iQxkNSZ)> zZ#nL5ww#UE4q^|K&!vl39^p}qb(-Sb2XnAp-Rf!o8J-Jbl|5Xf<9Fj&Ku^E2d;Agz zKRc60!$s>toXD`jet8BZNgmF)mB!#iJb&DO!WfX0^lcmS{5?-pro<4tItcN0W{G_+K+G6YP;1<|XcF=I?0SsMp;B`Cl zhlk2>3-M-vnB(L(r;k9yWpI^dW6tnW%by_VBnU!DKwfHXN@r3b0P7%Sr=DxUM1>oR zf3LsO{9<8k|H#CU=S|IPwWP0I5dw+w(l*a{N;KdPgV1=Jbs`|_$j;t6K~gq{Iev;y z{EaN&qKp&2+8_39LMj~^2b32_L9htg-4hRHcmM9*-Ge)St3^xJ4qfX@$Y|wyX4u#a z{w%A6WA*^cRlvF%sfgWl1RexyWktt-jKVkKU>vxYt3|ersR2ab$iez+D|5qqk01Aq zjxxa<94Ll<16l^fQjUa>Kp{aQE(XUHk+`2**G=tdSnjgdfk)&12r2+nyT#7$W27|& zsR>4MPB?T6&08`U8W^t35acfyai~Yy;$(;YZIfe@RDeWA6_{{I;w=QJHqUH140dXP z9~z(%%~lj(DxijdesE!WqW{^m=P{tiT0(3B$ixjj5=@&dp7!QieVh!m@9~4rA3uHk zEAM{NuccUv9?_z0bTd8>S8oT2nUpgkM|wRtC8PluS1~Ognr!Glm6x-|qzh!&lG^Y^ zpC#Rx^eRaRsD>+%&_Vhp*KVzEkfoKn*ZjjgdI>90oYD-xNIys$zBBq`{);w~Bo-YiTZq38>&#jIqAw0zW4=YR46TMHK^SqpC*-jW69i6c5et5!D@=LJIHs>+! z_w7<;^cnt+Dkc%IyJ`{iaq}i)IgqAgD@7{`ikNZ7EL&e8=Senet(p47=%lp|H~Ucc z_?{#p$rq>llh11!y93gkL@AJkZzn?Bm@CY;S?*}0LL^&sJ?P!Bp%F`^39wFJ^+2Pe7Ac-Y_kYeRh58>T7@(zN>fyRu76)Gt)0mH~|f;raG zZ=jc@7KMR$w^a*jc>|C=NE3d^fhCp!LYOtw246gR+S@-c@bvz@2T$Jr`#<;oa!pd*&l%c$1`)`8HsrP2E4HF3ZM1 zvB6njFH$in8xfMR0D2v+z){-Jb25sZ|3hk#n7xg;xHDn*nF&!8x~Oh6zLGHp1aVYy z<5pw_^$os6J7un*DN;4&W~$>O{T|TJSgkgTN#o)>!O5MEKbG6y#W_>v=o8wR>Lmcu zrMveFwWxE`WV#tE#wbD7(ln0T^GnH==PVUvl$4H!4a^KpjefnQsoLSs(m8^P zaX2!5S^UE|JG@1pj=nS)86WBfNdmhBU+TLGQFLl;JIg;=MvN+K8d_YhQ4Ug{UA`P1 zp}IX_b!JPAan;YTBsV_r^zpO)`@j5+doQLCswPeL$al_MymNDPso|26PERi`i__{! zarCz#w+^vm+6Lq4;hE(Sj$UQV9PJ-ES%EHkFHCTnU;Tm^hJ()%C%m0-ku!0ax-=YU zNvY|8GSmjZu_Qr*nmOM$dIN|5px9~2_7PCpHr_TS{%%aOyoD_qfL*@eXUz z+&|=8sPv(A!=Km9H)a-Ai^#F48N6E5Au-k zAn5!dZE!3I7aJ;!5J0OM94=X3%4QwUmwYWD9tzM2*I1WBF1z;;HYnwuCs8eqvkU?0 z9f!ueWL+0ad_K39qmivST`HWfH8jmo$-}zP%O3O2TB}z^hIr|WymD_nucP&h;xk2DXS39`#x?RTvvb9 z_`@TOJEL8Fh-o?&gqY||t7MGoLmDt{b}`UY{|P`S$$F=E&dosP&NZf5`A9@aB{SKK zAfQ;U2eP|F>x~6BK?8E69%ip33wFW|2>C*9?|sqNKRjq%=-FtbXV9B|$iR8L4$2(;78}{qYhJiS z0cn=)a3VJWB{*K9t2ZY`9dZvvd$AM>kyLUfL)j1tfMiZ2CxatiD%xoI*>eiz8%3LW zk5+JX8`8+!;G#79(=+N{AiYqko7SyZ7hZ5LPeUyR^(XwN7w+t~Dbv#hN!_u0;kD{{ zbFH@X$VR^j@hHsM^8Fp}%>|9>e`R8P%neOXdk4o&BPcP-ol=kfZ4l@yQ3Y#D@Poc4 zlEn=(;;Hemf#C^mkHsRLf9fO@0d1u;M<}3W;&9*L#%c7X7s(7iEIYsdrN3lM;T|Oo zTCD4FD0Y>t2dN#NPMejjAXLZGKm~_FshU(*!P&`8qDTUWI@84#Vn7#=T>mx(Ql7(7 z1A_{wH>2weOQsg+8bcd)B45{gD~*AUN5+%*2!umc33fO4PIec^`^{*<8|tx9eoAA# zPoBIOt<>a2os&|H@|zO^mzUSKH(!2neWfBO3Hv6EPHV_Xv1Ewds!aGI>+OH8Hmoo2 z?y?IourAy9ApFFD?+ET$EOhq+Z@3sEp-1o`hg4lqrm4H=O{a#72(a-O@L@tl;yXKk z`SR5luU@~pWoUfC|A+*J|M1G4H|uC~3;G$8sO%b3*~Xktqu@CE#h`>A<@qnzk}Tj23>8c9uS8Ivo){VK9nutHUESgw$ivKe zz2#u zK7M=lMWZO?Nm*WRi6>T1P2}_lQU-jM z1E%#7eTf2aG|Lc3PqLAu)ZMMU(}RubP<6I4Ha^aPb6{X_Wbn~vpFMihKQ_gCI6z(n ztxhk-x%}q#i<>WBxn|@Pzj?HOa+Y}<=`*4Y=&jZ#CT!urE7sMPx3)BZ*lDj%iTeLC z0a!y7Wo5#et07wOk&0(~oK0(@AneTY>ZPOT^UGu*2=qPXK6@7zuU`5Dw=ZAf|6FUt z5xm24Y~jv1j!R2}fSKob`vRaD>97w9o%xHx~TH?lN-J>)Omqp@aLV%aTC6zw!PB;2( zdq*-z{`EF85m~$feHXXXj9Am~2?36EM6O;N9(>Vj`^+(%oN4;%+67u@4U+;=MHH3w zo)D|Z@JVpNU5=Ee+TON>c(UhcdCFPXk>QK(n`lu&RO46hu)sl}^xg_Q5EqE_tn>-e z%VK?E#0~b9;lZ)NhaZ09<7W>aK4-H$+&^YvaE#F=#2$GA*spZ8q{WzkgW!Rc*$eQc zg`^HYYB5F!<#htD{)H6)<$b=WMZs2H*O>E5fg~# z>d+V?YXbFlZoTMQ={#LsaO-CF+x^5ycujuj68CnHe%b>4sZW3@-oEoAoim}W*&_~L zPZV+oMdSUh%uUPnk;B>1i|S`7)KC79-e~6_b%d|N#*b5QEZjLcs;oTcKsk9yFT{dG zfEgmt5jVr0cu`;tX)QQo<`8EoEMvSmeBEOCk09X;R{7+ht%H)&4MbiRifVP2{KH*G zR^doH$gxf4DidSFahM5sZ~u!IeM2J};@r5pvO_wVOqz!q zBSuIV`gK&D*WCk6NdB4_P*o)oIAPgfHFEuEt+lS|V6QlMlgc0qqqlw}MxCo(##1zitKd=lH7_G|^KZ5C% zwHfeifXS~#Eb{mWnOAnAY}^1;D{oBSYb;cTv( zLu({QS#k8X7C~1S%#NT4UF;(omt|Js?gH4^yWieuA~96|<@KJm|DF4g!vb0$iakC7 zw0l5)5L5)J!&%t1BuB$-30sm`bjnBp_^et7m@tqf(B$H)twz3)aL@}Ff@30i1*ZfO z--SB!3{Y((DTbrP`S*`l2zzWU!7wvBAjNX)q6jho^(v(V~Zmts`_`?q3m1t4L0yV0JrIbhFjzL@zD#>G~U zt3XD0f{#?AO3k4D|IrefZ$fvlma@9iS?MbMpw^vBS;$PB zTF#(8_tC`Bzk781)tA?Msfkfjm=8^QbCfYA|Fxg;=ZTpd_n(XNmsc0ZLSTmvMow=W zfG%z>&o8`uXQ+JCZ1;?lW#_nvfd;d3mI+it2$a`i`X3aiHEOb>AbXeydRI*_2oy9f z2a#Av5L^Vqm2C45sEypI>CnpgYniIkF{mws%04Ga7(H5!yO-8l6hdeGKQAxbQFFm4 z`0z~T&3&)_x(nGZZojxKUVh(gPd84-4a+xe{5i2E{aP|l2sGRv4l?VMw!Vn==t8y5 zYg_2a+5d@VtUBdDRDgmRA!Fa2pXFoZW$Be~K!yhj@M3x$DPr%J-7 zOajsBCJ8uFT1-?kfy=B2;Lm@GR)qNy0%2&qP5~TRB)F)vBLo1aV*8J?rA-Z?36#C! z&+XbSMIR+4QN`>sJ9`O%2$yn4#0q6kYt04J7cL5R#?P)@Lw+gCTm z)%Q=&ZmzGc&MzrWXtpkr3bLP1>aH@Ud(9{&0KZNc=b7M&*EbjDih9xdmqcSWOh^0g zKX~x)>A*NZFgw5Mdu91so0hf}1l+)4$%b$Lp}dgx#FR;U622j~t&WY)k)y6aD~W*8 zfe$!A<{h=5_!FexDQfY%r^iQD>vy)7h&hRrmb?zP5@KQpMloc3J>@4j+OCO>n^bi> z!bNR|xG5mHGTQg-dB4hmN_P=8GH^!(+<=mmDpNqhnA%j#PJ$K5Hp#J1^)KBuxP1=O zff^_#ucjuAM4mpy{y*y*smyxDIqK=IR7qgf%^pE>7w0{vKfvf>V+p!*3YhBVoICwOrr4;V1v+p1h4L`y?b7s8V=4kXC{0BCCf6zwy&UbfXGcdy zCTrtUE_@qv;bS^T^cFWslXDy7P?DmIMT|~dc0rgqoN`E*bbu36nN3GWRm*Qwzuj)t zvfea1U167Ny?OqRGCDWSaLs}52N>eiq_TOA98N##tK)Ku7plQn%<nJNuIOr<4Ubt#lgK=XcXuhGgAgY7D)UHy7V9&5RYZ&XS?gz4|lSoW1=^d z*&Uf;>y5_AXf^*u3D7Cx4K`6eJF9CBj_fE`+OxYj*`8Pa-G<|RkqRRbOD!V}Cyr3_ zZfnd_h{b-)ZSX*fH1MsoevB|k;;!_pe{_fQHP|-i%`}A?&5RKb>o4YC_1Wl`dX{a-&G!MJEz9>$b zjuRNW)9s099pMMu;k5xjvIfi-A-mTuI1oG%fM=4e4Ykw%6`KwJfer~_PrV{X_&)v| zo_Xs#MtRT6K7|df zX<64K(G?44x$9RqwXF?edf77mgwTJgR}0xD8scv*!e}L-?%awkFHQ`*Z>2NY8xnkI zBA#K;(!vTF+BfsDJ&$u{n3>~>2&g=Klu;qA+L?ya)k|T)xP6;Q>D|;Hu1zu?jXX>R zk`;S@LcYq?UXt|>#Dga#m9665vpve@H9HSRd6wJMa~qm&3&k-dlPA$K0&X>@@Fv`Q z`g*NQmO?{>+RXUK)ZKx*&+gz@q28FIac-CnOW7_A43_OYCIW1FG^Y#yYtl{8{CAe~ z^1FiPO{&wSM#hq&?GlT*sQTm?tQ3wZh4 zdNLijODm-=)Z|v@s5pNUbPo1eAc}pw zT{aU#UgLS2b8@j76tNKY6Rsso@~>F! zt3`UiBcPpo*6rn40SmoxaAX9#*AkDsJU>0~_|em6&q=RbBk?u_D6atHiBY-g@l!(_ z?tUYaQ-CaJz_ka(5$7i2Z-^n6jH{VOX5c8K$x6i-Dj+)Ti-~Nwg_a1ALUeDhh?p6r znmFK0N&)loJn8mq3on5oSnC+TVu3J$E@o*j7jx?_98F<7rQ86EKn=GRbblNs03SuR z9T#f1xoze@c27>+Pi(Iu!Vc7~WU=0w92%G)f6bgR9zwaZ7KYTU7?UB2moLF%1C3>Q5y>IG3!z|z#vX*V>XSN8?nts@mmju^AebJ4C;{%FNEQL7 z#o6MyhFez)_tI_mB;#+kt*hG_I60FS%i&d4e~u2`+52y@r053Nz1;?Eh~eCgVb%To;#7RY@Kn>OmXg7K&%qmoJ|@&w5ZAU1 zvAfLR$kx1nEx$7)UA7JC$7$f%_<2kM2%Q*oje+;MtT9lmHlW5``cq#gifi-Zdj|%9!3TF<&hH0IGWvr*{om@mm9M075qqi!31srMqhgau<0>9?S zo?Y&fYy6v1^A|qZ%C???|#GhmwUSCjo>9AT#;`YiMWjA7bJ1k4~mReFB zhz7SIXs2C2YDdp-cNPITx3*Z!{m8CvrguX_eSQ6ytSQ34sZ8Yftbr4^>bEq9jM_m8m7 zP(!*AsMHH-x^~267mlvnXmG5^?!{{E=EAOkhMLq_le`-Jw5;n}uE`>NPoA)~=S<}vB)Py@xuo71b?SGQNZVO_Iyw14x;#*a7_Dw}Z= z_V#(cn0+cCxzZ!p+%r}tlTnxyonJ}-Cv3{jk9XEZm@Q)WD`1PE95;_Z#Zgq*=xxM1 zYRwfW34wT2=KQtbB{GNMq5i)9qH<+Dbx(tPaW-#{V`Yon!=VTlGCJw*I`)9=juELC zD2}H=8xn)GjNUwddod8*v6E@G;JQ(l2OJ-fiupVjogY44~_!x6`xpv!Kr847Xou z&Iv8Ik2%pE;A$lih9P+Tqb+ylhymIc1J+3Y0DK&R(M`)GBDB3tetu3}g{gd_DA?sr0p#$pMHVl#PhKcw>st~n_g+U z`%ptBw^5dCDES5x(ppkglJ$AU`dwxF+N7?A@6=*F2N) zb;4NUny&fJDQ1naN^ITe`>8dsgBql!`wB%J8LV;{ckMI}YiE$Gb8mo1^Trx1KHY40 z)08^W(pkCvUu^$nmRjg-6~O(oo7cC&fOJkG`Y_|5+{z3yow{@OL=}`9QNv-J{QXc) zn}qdH22x~|)1nn$f<9{fW%WD1JKEk-Ow!>|yR}fvzT_>$xY({>lu?%C`BWK!F-~bt zOyrhA2&yYlx^Ax3DyG}EM|oC(Gf7T>h68@kv;R-Ch~bpKRXAhZ<09iBM7FK1*Oahk z*xKJ-3b95U-HMh^GwOgx_}5;Z85x{tNvAFZ);67A+INind2_|p&aBlYCnDYT-r{_6 zm>b%ieDjH5=Tj;1)li^9x`9f0Ew?r0eJP*ym zp3)9~#-r2go0quHI2a%7cc|C2l`K`KWi>ve+jr(8vyMtu6I0ap!Q`v}&R4h-o4l_{ zxw#&Skc>x+B}um`x@F62pv&4M8(*&(vv1ED;lvh@IvVCwBUHC+e;60&5uALGe>T-?UVg=csgE2E@F|U z&TlT+7Dtl0+87&}azTefl2>wdb9Q=JT>bd`XvZC;?j~!_M(=`EFZr}ruPzZ2Hi>2X z)AMU9)mN_$w!3r-{!H7zb&tD+m9H;uFYHVS1RB|@QTWWs*{(IC21w}Fztg?aVfu9^ zmnd&#<&;?E)yvYun*Q?ml%z8Do#`!Y7IyBugdZ@Uyb@V+&y*fnG)p4DT+>rEgd+$g z;x_kn=+`gpteBc0G9tzCEObjK0TqF`70)U^B?7kDP|R~>;>@Nz5NsU&U`Ic&Z*#bN zcyasH7al)K&JvikX8@9R*nH;ZsA~~cu8qMMa$0!UA9$^ns!I4=IDqxfJ_A`W%2knB zN5!qUoT>b^>>jobk2c-R{oB4$L!0JpNdmNL&84!kv)s|CW4p|UvcS$qol6WEWF%4! zKSOK6pbU(~ZuA9jB(HSU&LFMGRwTuKWWIHVI~nna%bjJj6VP&c0jaaKb9#ABBI^IP z*P0U}(^iTkvn;k;+K#(sOa*q$!!t31X*&gi=fjj)POufF2`308U+Mr8ll$eQ_Hhvox@`;SNP# zV`r;pyBd>;^*as67W+}m|6jSG37RE&n{EU;>YBs=%msMmi}Q$QMko>c8zvq(3|M}J z-!b$b@yJ9DXoBn<8G(X}UYHRZ#Q<_Jbcl8abYTv_vghcGWFd2U4J|k4a!o7d87s-` zE*oWFQiqnb(c3?y@H6_eALASEpPs+EJ&x?_LWn!F4#o)!Lx(i31H|UJ(Un@n9XP>> z#p3rD05jb^k&4XV%VB>_5LUOeu>0M^>l4VBL0Ra=5i;DeaOz-H@dt$rHOEPk$I2cWLAN;>^Z)BkekbadNN);q-DOI z%xg83`U`KY|GVV{;)PujWH+F-ZM$?Y&W_*Q(|_ASy=KzbuQJHR?*NX=&fG zMTID}1QUg(-BIRwXE(R!XIJqJ@PIXa{v6^}Mu#NYvJ*-y8()AKBBj_{k6pl#`v%Y}*%^=rezOTQ}*PKG-0rOWpZvgft~v_<6ZNdO^gclJD! z$muwk7gGy%BshIZWE_9(D~e@b+#eL279~2;h1|*i968xqtm9TPm!UIlZy#LTzBC7P zvmu$U#3&4LAleW|04cl|-KYwH#>QxSnQpSXz5U`#mn54X0MyJCJTd1Yo_L!qxzdCk zIGm#m(vI zHK&8OF(8%gyaE33_#IcY02w5bIq=8u!ikie9s<*jB~q);F1~nmqJJ;|HYkzXt z21p+g-y8PSO*6zm-=#{ZBujdz6R~Of}x0eu^Kd_%90-;4L7n55H#yfYe z)DFj8nkShies@E8W4xcy5*+0pE=%;jaWbC7Tw1~vTQ39CUBVm>9Aj`TkcS)*yc90X zmuA7I4J-+>(m7Xi{G+rHxYqJAVUNHeM7Lu4VI+_#TVLrR^!D;}OPWouK`iJ1EA^et zo}JgygbxY9GMsw}<=kBOw~|xHUXFqlEfLA>%#4e1Qdkyc6!?ujhF32IED{8wTXqjnF?D8 zb7$jS_m;*QEI1-nrf65!iq7|?Nl&Ch1CZ4zW8qnbC7z(#+g>+Vh_pe)kn|I{hM!J? zs!{CL6GG5bwOlLB?nHZ4LDsYHpA=`c^V7ShYzDp*;oeBPNI5A|-qluRXgcNzp}#aV zSwYeQ`&_Bqtj6|VVz5-26FrU{wzQwox0LD^trA0QnZu@DXV=SQMff_(uBg$pu$hnw zA5YJB@>wuNX|6OpR?7ep=0;Y&wtwn;Apy}ZFl0$D#zVlwNaUWrx_Nc;^72ZbBMdI} z+^94G8)cbc1}g!%Qd z9CRBOF2dHkq#mBl8QL^WIs1XeYxmHKz`3hVB}hZF6zm|03JaMyngk?b@G71$^5MhBflMd(akK&a=X;30(!}svd|C%#wRiF5BaaH z|66MCT#;t%cL+?Lo$m+uc5nXSuYUI`mpPbC9-O>-b$jzFLGbdXoFc1U-PwgCpd$e0=A zotLnIU%I-!$yDV;H;kVEj=;AP!65-fUC#2dpUyVSOAsDgd-{J(2zCjGQ2np`H=nT3 zUx?fj4WzKj9fYxyaSvv6q?A)-47i%&?#Z25Bxv$|vGLk7vfT>vw+$~mQ&+KyK7-9{ zzKS^Ll(Z+c!51;dC^nWIVcW0BS){S&T(oa;fIVk>Q`spnGRW&M<4LNBggw1FS<+D` z6U6|?kVcGYwn@E=x&yiJ^Z!Vr=0Au(pFtRSuQHP}!1GLx<+=vU;?+fDNiva%zR_)l zqM1`h3<{W3;{$D<#1(5tDGlq z>j8vp?{%pORgOeUNR;q5u;p5CwAi6A4o1wkWNHisTGXV2kq1LE_>G8DNy2}&CN3&vmM^UfNw}%Jx@5i5;X1$upo+$=+(gppEG=*L zZ2uKR4K~k<{1tB^T)DJ-bo`P$&pa=9`|RqwijC8BeEIsT@0{P>g5zh_ZMS^_=)TtI z-hL@Sayp&qj1!$|k*E;N*`Tkm(zX_xo|oeHubiBt3wK6pfa4U5TD3`llQ1Y+^Il1D zu=1Hw*@?nCvY4eC2^Z~I`3+GPFjHnzzpzlBa5a}c8)TE>bc|}4H*G`4fq?$b;Cxq+ z!To2Ze-QK-+GfAzxSFjWCqDU|U|E-0-J^%6G2C}Ka z{v@Ni^8II|hw$_7amIv*_5C0sbtH%EB? z6lLH*3xL1%4~&*}T>(h->H&KLV2gt-nxZtpsNj-)25&amz`}lRVSIV>${RB;@q~mx z@QCW48J-T>x6!Ofny{h}pmKE*{LkAvymUFHszdu>g?8=X*`5;vt7bW8XEX`a_h1iE z?A79h-(uyv_w8UZ#Fk(T6?d*d#m~LjW#<#j0qLva*M`%sHh36 z()xO&jfyd}=9`?}ynab%?a0nAyv~lSze<|GZP>hWa>`XGnBsWh=pZ!x5W6#$RWJ}- zvV0W);JPk;wo_T9MP+DhG-X|ua>&?rM+aqfG?OTlD$h#qEDR+!iTF?ZrMCIOY~D_a z)7g{Do7dO3F9pHHo$HLt^&aGShr8M82(SkLOQ$!Pz$62y>Ja6+!^S27{Cs@#J2oE| ziI-5YZgyNV6w8)G$}yO=Te#GMs3KD&w>_jIjc0|+piKDO-_1!e)L51cNGbdKEA-5eDX^Zcsi<=MUcBhV_V5j+ zi@-LRg>w#dbLY_Gd!*W8JQgq3wG5oxLRKoaWOBMPw~@L0G_y+$_gc?-2m)C&CYm-m zqh{sK#0K!7eB_%uQ82+C97IX^3uPmAe{NG-dN8i;;rKw$#MwF_E9295)FI;@J^oz0! ze_VHF+QPF>A`Y_EWCJmJQZ$yj;>ee4GIgnHt&XA(pA! z+O*Ls`D3zemmWW=*s-bj0V z9DL+6`(F*?y&pwAmkUEP6ak(g{c3Xu_A86e7UL~O_BBsj;1cMn}v_Y?IBRhaL6dXy=gK3XnD8~A4uG5=2)By@w7XfqNt zbO4r@?U+E_iGj7mK=57U(Kt!Yx!Xti88n_{3`pbIf_FUzY$IcnrylG3zUSwAvzZfT z!?r{(Dgv5mPtGMI2^q}m6GOwzAwp!>8?Zk9BTTVgN7thy2{(1b=v+<=e}RO|^ro|g z=yH+ws#WA&N-;fw(Ka0{fjr?4Oh{7M*gM;vpB4n}z$*s+q_(4M8fd-kM!*l}H_fi*4#+p3~z`Kd&upnChY1eaTgQi0dw!=qEYJHHeHQr|OpZJWqgzkyrN* zOZ|*laA|&3#?`Lp!mkJth8vOuU>O-bPu~70j3KKgLR{>aOaou16#A3tzzI29S7N|& z!0gN-)_HyB znJ5+&jEjx6R_A&MdO{pocmp=HFb*we5Wm9*vgU8GXYQ^iBGm*uUk5 z;O6|ytgZ5&NE*zOId~tCFz(^xl1WW#^cgPhA>aSm)j>j8gzvLIWN$RxO{(= zI%5Wq09?u=?zl#xq>(h(-mONVNF;7Yr0Y0)b zMWx^~I>dUw!nL!%9}hhX)BcEX$BRm8HvJi7WsAtT&*(L)6M>g8Kf$BGed^gt7K15J zL4ML|Ds?mOy~B47ew;&}OJ*Ep`2m5POa{R2Z66#arxTTMN)q9!vEv&`YaWG& z(a5@}eRDgLzkQnsAkfabIRHi?E+gro{f_eWTXQb9Z;?bwA*}%^eDFQV#X{~#hiv>f z??R0*bCU$XUGKkhC#+1U&A=W5tcWgB14_A+P-1JL*~)5|Hk0n%yFdtd=C)srtwDh( zV_b9^wq?mUT!16hxLxG?LTV}K$#edsT)~%LL_2S(b~2L{6&E_-xdTdI)bN9Ho=T#Y zm2^u8K18R|jOjKv-szf>;>CN8HnpaEB&+sW%kP!`BYJPx~rD zz#ZSY7{tAB(^C^O%aYf4|G@Cz;6VS-Xk~R4E9(_9`bk#-Z`n;)C6=r(0jo?~SDv!6 zu8Xr=;bsJ9w^-hpqoi~11m>g+0gCK&c@jIsXF9RKO0rUm12LYv+$R*VzEyGjA5J{{ z@Fki7FOo=3IAeB`y0YoM;#H>|h7Z+aZ^XXZ<_@q91DwhL2ivQutQJgSmI4C%KyO2! z)S|ifIq=E^TnISV+ndYo`GzNp$a`&1Wd+k1QXuZ_-rbb~dfKnS|0pnXac&s^VR(#C z<477`Et4Jvg?F9A+m2U9vGHZSW|72vo%YmC^}>?&+V;89^EE{0X=4p zk7sa@b0{U{Ev^!~cZ%)TT6#I0q6RA>tnfNX*uo}D69P)~hSx5Ak3$mmY<*Ai&vO%l zo|X2HE0MS0HQMjO&xsHHOOPy$j@NB93D$L4Yjd?#7hQ|1?`*tr%>getNhl-O*ucn> z_(iO}-)BV?w^jCuj!68ngh4Q#)*Jp14rwnzOKk&czDtA-m&#NbLawtLa7Fysz~lQ5 zJ{y=A9UdJW8XOqTNVYc7_vCr+^M@aP_1^oRJ>kYMZ+AU2zs02(AxKnkwgimODDP0C zrsO)F%)iFU>R3{AkWD@hO2|xb;xFXb;wXl5F;=p+>peJD>0$l z4`8az9`H)R^S!!mS@5(rLwpwp3u)uiMCY#-OMlG6Pg)F0@)q2-7MirXdv@Ty1=ljc z&m1%OQiK5%m#V8p@WTTmf|Fz30kVK#%Wi-EnlcozE~;3cRnCXMBQ+s)gbux(=*;{Q z7n^BH;GZ{IvOnn_kO^54Ll1WOyoSR~Na8@f+uXtEf~5AT?Z;O5`wfyI)lmzBiKmrxwwSyHHt{Pko#ez zCc@e5oq@`}8IeU2hS#HhxW*{A%}ww;zW?A}-{kn%1P!_CqTF%{13v2;5CXsQ{%3ZJ zhA^;@uEos7kU6T_g83rSC-k0OkVOVrQzWuT?zgh|ls9=>brSt_HPT0}Rig~Py-`c|~gLLoc%3LLIDyv{8k{^tB)(wzVi|Bwb?a@&x3`?Y| zwS^`Ty>lW0q75vANC7PG?Q*>QshE@c#$wGI);@>c06bS*BpX=IT~pIwn3n^To5M3` z6162vvuBSUJbnTUjyU(rk}1@x=iZXVbYW?U^WE2amWy?FlF-Th z#6%~d;3IkimI~6t`|OhW-s~r&tdtNi z{*bvl|1dX9xiJ5N0Z^h1a4!=t0+2n<#|G|5uK z$o`!&mmySTUrLwsZK{{h6a0k1es_9QD~LsI8wDf9q(kn17G=JxMi3~|+ZF9!(-I*R zZNQ!CXLSA9hY}+Z97k{fV+a>Sl0!y}k_Q)Z&YvjO=3l6?g8SkGoppo6EY{>pxF@|#(evNE04>eOyl;0i=iywoPOgzHoW{+vCroLBZriO<5o_+rC>C?}j zDd`Vhj81beCV+B&imiX;*SYPXBIR4Mud^Ui{t2?Q@=cvU|C8sO7+C%G_dWagy-)ha zrkXABZJLP=(0;qTOq$ZDZD{UPNEOhnQSsK1OTm$)-<5+MQTa9vxl%}g6un=#to*t9 zTxFzxEL~294Y5x3csWq;Lxc|9&Y24XEVB51JsCd8CWNpWd9RlYIfFCFx9I+9C|G$9 zJd7SG5G`*yCk8S?Dr}INWc|}GAer_Z#H_U9@#m+?-Y~9#fLB^}644h?rtt)@j&ygL z!=Q_5Br}O9L*y*(|F!SwD-wDZb;Th`So#W)Tb!^}S}k_|l@F{v!TifMCMo!3tNGWOV4SO zdnf?tMdN&efN6uXW0`rcrda#NhpcC(N}TgO&)25a;i1uVcQgHEYy?T^jpMXO!q=dp z?A)*lC>fpop@?zJWuJ^_Jbmq_BP^tn ztRnlcH>_g-TfjD71Ds2{MA=}ABgb3}*nzc4<}}LeBGz9tjmrPvU*YXUwPXf+-Vd$jrfFUsq>i30xV7r!a4Oz+tgYcHpuC=1-+Y|Tnct;8X zA(zj$inrypBmpqjGJMUYF%~W7!TY}ciSZv+<|kbjFgn;fXd$}YR)($jTmaR%zf0DY z&knccM$gdTQJ3oRWNj!xP^`VoGl2ZPcKWyQMXIyJUIxKh{2B(52o`xY%o~QLs^D%M zFO@ySi3#3U-ee5afg_RR_CdXx9019mc!iNUc!+8%&4Td>5&M3(XZv?EGB;ne_rhR^ z0As~YJQ*Z>7`-kczz^f}rYq{AQn~JWgC$d+X$&ei0`;tk%rD(tVmSp!8it9 zm*#vp#}VM+vO=OwydLH{H6d^vHc$laUak?oZ(SP{m$yk=W z{7o$sDH_e`_E#kWjV!Q=Y;nnh6C(3_jJf{D86kgZE?{6xSWQ-)9h-~)UcRXg^*;OL zqmS=>{L#Hfk3aq3lTW_=?l1oQKlr(y|G8iM+It^8?44}rok2`%Y0jr_t4$T|Ja%{m zSg>vJJGJsVG1zzin}6@`efY3%vGVx+k3aau|MDOF;=A{r3{)mYM=S1M&E&!sjQaV; zKBlWanMt+R8|$JPu1=;6XBN)h{h#qCN`77l z8dX43AQ*p>L2AKVUA3{wz%*NyJK#=^Vb(W--c>GXh@egRXw>f3@3!vzfdNs{hWw?h z=}Y_yHJgg(KLr8jkCa%MQMi`PyNC%ZnSe|9mO)^l+81(8iuR#%5IR{T2!bp=Skp@o zpstYBvH$<`ES6<&f1X!gx)C@!*+GywQ(6T|TK=OOYxA*T^=LZ2nP$CF8y)O_`1zwJ zpWlDfH}Le~lSiL^_};gE`Ip{(_gB9C+2ejUKXkTm3FlX*d~t}?i_WjdCAFj?+Ufv5 zm{}VV7Bl-k`Q@)acs|x?4t@UQ;rqYzt#5zwe5gYBtyOXoa-Sy4q%Tr)HSUHrK+XJ? z{Y@reo-yideI?q^rN|UaFpVp)GsWiNj;(HMB)B~5{))dBEOn@D5``u1b;1dnhWUou z(80toqTxVbE4dXK{$Ei^e(ok;BKuU_4(*X$9CahGQVd+U$1WO6cYGOPj6|)LglcE( z5%3jCJL}!vi~)hF61pqrQ4$XZ^Jg;E^9Lb9yQY@#B!ZPYzm}fLUn2YQ?`5oAOl zz>3|!ad5i19DR--y&;p5VXejFv&1qUiR4x;k~%w5ES+&mT}LT%8Tdnd?xP2fdix(g zc=YJAk3W3>TkpR2?T;N5S3_*w7B6a{yPSi`j_+Kb(5UzfbV^gEM`g)OEY;Ij87P}f%i z{uxe*0US|J_pkjrIjG}h3xW7eH)APj^6TnbT{MR|UyO)$vx6;g11Wv5H!oj0bTWwt zrSybxqX>voBLE*)o;c!$FDgUe%s{{|yl_MP?th(eSQ!G+pPwxDJT((1fa|Ec=?bRg zX}CgB4yIFaWoOA4{3Ovap<7+NyJCGTOeZlB8rdzj{z{p}GoI3=bHtKWMK~>;Yeh69 zPBzPseBf;}+wC?bfDeLz)-UgM*CKsMsz5zm@;m>Er1$K+ID!BozawfFy^w9#7u|*u zCHoaC|0Ek`$ZHg~+ddF1#@?LqN+ZJE}<17yn}~*VT#1+ME=fk{cYq?s{$GYcq_Q&)@syuYL3VCnLkd1J53O_|0#= z|H;tw$fFOw{$Kt5pa0s|e&v(<&xwNW-+S8E%Zz5q9btYgd^$Hrn$upids=MpEGBx1 zCdM>97eZTY@P>%+l%7!HCKgNoF07wvxR34+2nls4UfAwUaEr8S$XFmKfzeXiXwm$q zpR(bCl;jm=nn3q66~9W+>qpKGe)ie(iI}{Oxq%Y9LobYkLT@=g#frsW=*k>8y#aTi zIiqlW!W0hzh=>b!)7OZ@>DtfgZKTpz#0{X!$L>zuoxO7daystZBCn**0D}kv(el`} z$d0q2#pltb3%lK(-KFp%Ql8v$XwY5cKJ`PnDSGsiU1&pCq-TUy<2gBAqCkCE; z_Q41D9#!V6eGlII#b5Z^FMs3x`;UjlMh73=f6j`=Elcjok(A)%<<&ik{WbTiM-Os- zc6~uZAv{Ay&1wcBcTw)lxvi49b2G<1V=>JmeH3cqpSm=-j0vvhs8}Hc6 z1g#MnbS32+Aqum)Wt&};a`x62CmucOkKBw07C*;m^4qw|*P62u+;KA7+)Zw?SMND7 zH87~GH&%UmXCNTMg&vC6I~xqb-6+Cxc5E#7=-!=I0kk33=d~i&*F0F`T;w&!{$js4 zT*kVcS!TN^-!_qn6|bJX77jnEn7DNR@c4{bGVFTn)muifho9A}^NW{X<<(mfwf96Z zkfWbbpN7D{V_uSy-dU?hF`)P{%+HI5XEPEvN(8Jo9en1|YRWR+`HeBjbQR6R+J9`A zy)yTp&p-Rj$^Yo12T$0}507XEJW?hRLQKxvSZQ}R9saDkZLJMIqX>C8%v@r$|KZ2q z`qqc{$6J->_ul=vpZkTc|I)YbKOP<*AAR-+O;j-{Ue&4j3Fxd9B*9*1X=B+x;kS5l zr6ciJ<81HcWTXXhpAtl82Y0JetPfd+RMR3>?v(Cf%XXk*nNeQ@c77x3vp+(g6^F_Lv3;J>UQP^A3f@uFc2w=h3szp zA9`Yap*A)+IKAMuxI9&@(wYAu#*o?|%^<3pJ|V;5M%)=1go-=6W|D?F))a2+bXZn7 zKcq~)wumYFt4&8c9--2*vsVHazi-G;ETtfKe%_F<9>f%|3L4H-e<1EkhzCO#wy&=R##bnRa}14+xzVEd*A;0 z*S_(s58uE4d{E)&dv^cB55D#7M<2iY3txZl{cjW>*53QLc=X+8!4#i;e~ZK z0xCXpT<-D%nMrY_`{JutoXOZqo%=5kC5_|>j^luA9)l~dI6Ea;G#u@gRbDKxjPns6=F2Mv$OS?ah>6l&)Leo=C@3FYr&EJ$M-(|_D9b@d;c5X{NTfPKl<>)_dk95{8{g_ zM^BzU`1H}UNpHbLeJ}{)8GgZT-jNuZbqc`HXTm-tAEy<2e3}BdEc)7YL{>=9Osc$m z`!Er_A`CFi2)CdcfdRvmF_5Y#iKAb(xv$L?@)_Vv3FuRNk zqm_1NcV{Q`%yjO)Ip_EbH|Lyl?zpQJtb~LRvJ7SgCW8>R0Ao=`6b4*Q6m`p==k29t zcXqa?`~KeF`#s+ip7WgN*p^K1;l|y*dh;4mpZroF$5GIW6Kb8(#=31lGSMAXtFi9O z;%52o_4{9Y_vWp5aCU{?U)=mg^nuQ7UB3fJPX*{i!8wv zv^1$)wp-YVtcQs;K{(s?3KKeGfSi{Zw|J1U^Jcg~Iv zz{%AK8GE6@#$Aze)ajctcex0wKX9Q zVC0AVCk`#}?(5UF`(U3m7*RE)z$I4`YOuI~ULd_zBRNlIki8_TnRG!!f0ApGY9*j- z!q*~EF7~^))5=`TA1Ov)=QQB*&-pJ5Be{SCF((_CPAxiJn<^Hj0I;fk`?loa(xqts z5UTO&G^rPfryTjp&`CH1G$pgFKpeSBFlH&29;9iZ?R}^y2?Lts<6{#Ol?OA7;P**? z1g<$;8g6;n)IK;~T?qe>6IPhGckm;(rn48L4poX_>V3QEhx;;$iODL;b;sk+KQDfC z|LA#B`)~~=A@}9X(?oSyyoUg6hnHl&-F#Tqse&OJHEW~2-2($PQjG4LD2SXrYe8A| z?nRfQx)k+5tp=<@s!Yd<8}$P?Az+=j8!TU|x;jimzqxntf3y>ez@r6VvsTIr6XT#YkONuYkOB;Uw2<`UvK}wNHKme zF*?xR-QP1bDrQbCYX8djX%`ZAnwu&nYH-G>#_10NLlxCQhOJc4^yQ`T9Avy_yCq}H z#4^N`WQ;GSIgUo~c7}e?v@RtLN6tnNi-vuiq&H4$JBgO z56~jvhhNtc7BXPo*tQu!r)!?2aoP`Dlqbb0nSPyx9;NQ5+6tVKVNm4^gZ9^KM41gM zPO)B$_6HS}z+V>gF)+{0*2g>a%SS4;CH*Lt$-(VAM#T!4+rHHa#Na4f(IDr>QAVV- zvshIV{Y{U){p#}{ef{KR+fc1oxc9SFb?HL-k8lwN`|0f)$$`<45bK&tf0XT)CbHA!3B8KTy5IlhfPqHNZA8F9j1|W3t}G8sv5I%IMI@nCCF>7**#gh zxH*!^rR6FfJNYAFLqZdDc|~%Iy9we5Wh*)B*;R3|Wn38~&V5=ghGj{<54@>Y6D|dK zKvEj75>^J+zDdJj`)pmOr$!u@*so#}pqsQ$-sf3l5y~n18_rd-p>c(jk;YMSZ9C#< zgqC?@N|oscQ~ZF*4^M+Xv{mWi3cbult%N^_-W^JtrZ?8Te(S(cWo~)HejyzFFcB{= z>~%=NvN}6~j`Q@*Q zot@GNsgmTOrK7jcVk?V4Q6gmFKpy^zATc}Owp@I|ffe0H<}OWhF++S=w>0^9>g?)J z8CV47b@>UuD-Uc~Qa%3LmFPBQzRJD_Y-y!;Kz;{*F1jE+6VyZ*gdcdBXQV`rP2q8>F!o7$$8uHunJg-v3iOrsndMNZaRw>HKJ}#L9S~YEr=t~W z`}QB>$V`v*ws#E>9h`s`OiCd7JK8(CX}3Dl$%zT=0M1|pk3_}~bD5}4)NOw+x$kME}?UTgA3>{&hQOimU@FduG z^-HVVRT9f+&Q^~Bobkl9Ee$weC_$TyH|w_)<#IFyrdS}dr7D?04we>;nvs4=&ukpFn!M5j*o<4i=s&ims3bn27 zZZ7dczpsS?9(9LQrJ@H_xj1vG7t6EWJ?eH((~}o1okNwF2n!bzW)>iVnTE4cY{qt% z)0^VxuciIA4p!1kped*wzI6z2x~BGoSCwrbP}dA0K(eqcQpSYf2;9`|o*wP*ADJx1 zf4>(s9Aj_*=+`$fC$E$|PgTdq{VjJ%<;{K65PAS>!PW8**#xVv3hj^(;z*I$)z&no zvQ(AVJAc1L1atlZfjfa?YKj+EN<-)49o}4Soc;~jSqP(kr?eil%9hxZLPJq-pjf)O z5ZFAh#bD>aGklJ8Riw$q-roVFt0tIsaA++H$^vDMCzc?m#%p?cw6n!d>$?FeJ1Uh> z!*W~VpNyDhbXGvZ(?$Lhq!$;{9!Z*@J2V9xIGCPV-i=_v-oYk4lV6xX*|*ePp|xr{ zgJ%86(AbcAawOX}3G9B6y~sbmF&Ui&vJ=ELcazY9!nX zrqJC?KEKKamKyF2A;J4OMwLWNI%~o`jr@ecsaFWyXUOx+Sj`+!Hrc0IV4$O|FQZd< zkt_9qp^;&9EGHGBEU6QIB}8wWt+9_9nl5SCJJ8$Yzr4i^OHNOY4vl4-8q@=s(M`zS zy?^5hNhjl0U3wH>Fh*zRP<94*M9VXW`?>}v@rlrEbx@@n>R)!Bp8fh!w9wMu!pB5M zq_SL_cQ<^kpnFdBtTGLGOs4I0|g%dV#5M;-WIAP zN%7E9q>r)K2S`yQ2g%Fi=JXAyMv_}wSgc^uTBA5J(9zoVp!>lhjVC`3ghaE;o>eo;p)v?x=C!r2X}WKu?-hjw+`9D@{o$crrLWC( zLZU;l5?uBPd}GeSX|eQUF~X_YYp6TDl!YAC`r@1#$t%yRbi4IH5vF!5_`ERfv(vcu zr|d8@xaD2Bx$y4tRlN<>s}otAle!6^yoRC`st>W*GH~rigvsYmNojo9G80axRG&B0 zw~#_O+`J9FUmqXJPOP?N0jSh|1bhtR3L(JY4n<#7=m$=t)d6MjCJvtO824J_*Rx0GD;BE~L+i62h zM^#Mv7ec`pz27^yfG*10=cF_V7S;0gViArA$Iv+k&2?&}S zSO9&@R3(q{Jf7SuWdg-YG{rYImR92j;DHR6DWogjc=)#nAz}yT^l)lGiQam-sY7<_ zTL<6qJ{+p&2(_QOw>?S^h1P zQ=ZAh)QToRv;QkU6-{*CEI0=XFSh*0ZKY|qLK!GM<{9cNXR5>_x0+c z_0C@7)=DU3J~$~5SS;$P_dwX)Nb z!wyr)sO-uLr|K>vv3O}hB7l0$#mQTBrJT=Ti=tfTtf@_bToJ5HuOIP3C(_O|{N3n4 zZSnM+kgMTqX){+Rl?7uBAM}|6!FywKcKb1FjzL=oD-GC7-uUuF(~KE~xG|?}EvD{k zd|S3xYh8jW@p2{RE{T{A@!*zbHufVWeHuR%-2~6zt{`5QG(a=YdRKe9rQLh);!i#m z5+Gjqn_XaK8(o~6sSk$%TtVWJRs#_i1-1LIF}-GOr<1T(RUT3`#pE;s1h74@sKWtr z3Z)<;YZD_+0)x3d$Ftj#ig=B6;ZxJc#avvTmnZX1`uQ^1^!qIv3mjlSTZ98ZK)Pf3 zc@S&aM(}&hGXG1pT+rhJ3UN$b&wp#19D;j+Yp^ZKQ3Z-&u~JDf@xeXeoGBkgN5kAF zPbSk+NQyK>k#XAWmW_c65m=w*r~Zm};y5WErNCzelKt3Wl7me_+$33;UMow$y{^IG z!NFk)21{!hB(kIn)nn0{QV_JWh=C0NuH`7`DUF9e zp>BhE1Q9uim;30Cja#hbH7?8-%hze) z*zhC%8z5(-ObPbTWRT)p{FbdCl$oB&+7J9vCYHI5+j%hiuKstx}FW;1& zO$=Ic**`c^9UmGR=^v|DTrgYI{zcG#tLnl($zV(tYDE9T97~{+V5j^oj|UXV`%>1Y zvQLMFw+e?mVN=5)2{=zDLQmmpM*BVqxj0v>{$qHPDfMlkFxw2hs8j{$aE5SPRIiE-c)FCpY#Szmc@tZ&{_|36O&9W?svw zNOiRv>YBZni=3Zi6iKdREgMv~Y7cTOt^=W#=0MjoQ{qb8mrjYZly8`hX?gYZ>+k*Y zcfRwh-~a51N&2)s1}18X7(9TZ)0!5p?%yi+ZuB_gMkSYQZ}-7A8u_XVocaMnV6vuu zT;FWmscfw^uKfK-Wz>h;o_ziI@$;8$T~PG#WVcQ?Cg5>qYNLIfUHwC&wp)*2%}+7` z>9!oJHnoC|f+-T3^QHFT4ShDFTE*c9d9;q_oCEN_u+WkLb5-GeV-ZP5*n#J9_+i(s z%E0!OGt1aRjuT$4_RB1`?#;snsUn$<);#1|KLl4@?y!jFID(WFUuxjck) zK?Y!1N}`Y9s=wn<)?GrMymbo|+hFYkBebiywaWqc0wQ+l(@Y_+lH%y6>5# zE?D5v$RuWQ`4K!zqc5-8)`5+Nnm8&{{p*cIj@>_t+rECuQMQr7iloWjrl-%Jy=sE9 z_7h5Xu)jF|7J#`s$U7CbRVglPj zMFnvQnn*0A+bFhv-RWk)iz;Lwj_h_YiNVTBB!_{)31aTcaT8E9kccVnV(aJf=h#^W z+{0^O81Voi%x?Pcj=wzpNF`)oj;e`2$2R<`ZzEht33L0y2Q4;c^PAH__LGav-xoA? zb~9FHM%-`TKQgY(>X{6QT^VM^llW}SHK0%l3ocE~nDz`I7=S~a6z#8Aw?rxlXR-9# z8Upi`;qK<9=Z}8${U3hz`NMBphQ?)S&}d7B2u*1MMm${-CTk$m#?{KYVgfrm#z~hO z?$bQXt{%Illm_aclQY{)`2a0Ih%iMedx&UTX$W}>Zlfwn(aqYS zuZ1EbdevHh<@Z6xDw76$cq9wR8bB75eddPdN(0eM@vb;)-RaDJ3+`$JR0Nq?O;ca} zX!~z^VwznS=Mmq%w0*bOpT(4i!*KwGwG6maLckik;BrbhV4q&IaSzc|i5<37x|=vhT1`U(q{kFhP`#O27QWjW zT}=KPT}W@<3Mfz;*M#GoZlGDjsDHWi^D|?k;WEJ1fM>4TBFS_e|L8O6XI(xwMMVCv|+tM%zzRk<0eB0CM2`qBFJ>Z(aQ^jtd8AVD$| zl$y`0!sEs!i^|%x=>{Q9B7wxGfs+xa-&;yUT-Al)gr{GI5?lL3)>%)sv447X{rWZK zTda{gPUuXf;jZ(A-bLJLidghRt~p~zB}^t+l@Fx1=|Jf|rW+|_SQ&(?T?X_-UB!Sb z7g_a)Na_BfJelJa0~W}N6vvBM4$qe?Y2%}+AI^dbg2A7jHy$lzy9g5VEyfg-x$)p1 z(OvdVlC(&z&>P+MEeNailCTW$nCbqgtCDcVh}GrHdgzo4rd#+Ov`P8x5zW-dQ2Q49 zC&9odqh9Snstv+pN?AIir6G1CIh~st9<;mKl}WR6rkMSIgBS__u;hh|q%ETh;NJ>} ztaxr_6JF|>M81{9YE{C^^SMRoDSOLv>Np$MM*7>IKcS=b>Sb#;Mjb+3+sIT_ZpI6i zE8RRqg5U*`Xy}|N5=ub<1`w41rH?hUsar*bvQg!wBWr3#nmwE-s(*t4$RqiHY_6MA zkY?a^z5?5usLP2kYUZG!Do!l~yTR)AcpshJ+`hF`==IyzB;v8gGV5RLFNv~o1X8)^ zNEgR7WJ3!kek$^nESI!6coGk@MkP>e!q}h&D|1vSm4qTbAoC~$*otkoSb|y@J0N*! zS#k6`rq7avFU*1|cF#pmiq4NVfGX~LudVA9D<)+Ks?wSI#$KRA@rx5q87o;18-Wct zHxNCctFfW}?$#&8H~WvCJ!?jH>1cV?)Hya|yzAqNMO=-G;(X=u(o3U4qL;>24-U+g z?>4*{cgH6ph3Bpzei?l;{q6;GLh5zzU~9$3wkXFCt~^;kJBf-%niXmj=`*~yaqkb3 z&n0*vdeX%8X7qj;5!A|U4oG9kj=<{fi*}1TBq^#f%|l5QNMQsVE~miowbc(+wY~7@ zmKS*@qztp18PgCSgJYtpTc`zn)J{?13B)dI2?9t8;(&K03R}hcb;uIYn)Qghf50>@ z(A0yTn6d6F%CWdCXBUmv_u{0H87nZ)sm!X-)B4Gufu(G3>5w>oe2ViEiHiKvkCz9t zMren!pIwW%@~1mXw1^uZ#GMI#2}RTwtM-?V&B#wmYi-03FN+ERt%>oRww6jTbHjTN!c4H$0eO>qt1eE%R zN84UJdGz%+FMH(f5k)bK!@#9nk&2(q*nEJv13Soe8h*!4bpl9&g{Dp#wzE@|3VmPv z2ZN;?oBXWQ7$J^ru1O3%%VA@j%BrXx9M_XO3!x~+#LVGcTYKgkR=97~Lb7aP0lRM` zQpUnVA7{$CpTR_3H?wlIA{~S!WY88KSMcABik|=T{bm$h)9Fk;S z-QApowl2xI2qG$tgwG;l@{pX1Nf?S2$|*=^C{Cw^t7y{4v}raUraFh>LFx*>@Br-+ff2qB>T%SMYaVbu}w{*R~rQ0FoH|y-)~DIIEK(-Q{lh0p@8K7 zjX5^t>2ozXD22j+{bWe@8r(QyN4qhCayZ4+P_#0K>FHwXUq`T)<96Ee_t67)K5`w1Wj}!ZJ#k>^z8scbOa@tg{68jpDH4sQ2#C!8{6DSb!UO z3NcZy4XP8+TeFbJr;SeVG+riahzO+e0k*KP08gXAe&T7JZ4!RSuTbUC4*~$BC`%Td zzC;X7K74X!1|Sv?ZKboDx7Byo=ZzP0*SO8rK$_7m&i@o}0k?>nfuB)Y!2Z5cO7i&8 z0+KY8c?a{7GI%N33?#lL$h~0!*9=1CtNtNOt+J&`D`8tR8JGnW2CnRyklS*xg%vHa z0E)5r3Q(;$hGPD=dCy2n0$3$!3Qlbkn4?T`V|jLBvc9mjG99WV3xHBYa1^FEU6l;< zw6}E>T}OR`V-xMKUf9~+&9kVaqCp{Ry7VSA)y`(@u28_99Ek$^4aLm89Hhk9%dQzm z5MXd$+Bz}N)!t``$_&(OGo*1~wr~`ibQ4_${}?5cgp@?^az~&C1!e^JRX4#%gv?Fw zEt4k0pormW(BKUn$IU}>l}WGl4X0{gf3$c2tl<}4W&aRvo}FecwQ7nQr=Pd8*+?Eq z*NOviUS^%M)!YzM$U{W{4ZDto<1lSbq(6q>|@dtte_oWb(DBK4fj80Fe8Uh>eg0bA}TnwI{&UKzpzNi zHz21Vd3d~SmeyRn?ZS=onk3R=98B)2syT-K1GhmGbQ7-l;4YmGt zJ{zH15$;1^VOjPrOLLlFW-I`}yNVviz`_bU`dt0HH+QmC>FVXxPk#OVdmH|&wL1P} zf8GabZAQ^UUVonn25BBE6KbTvKm~{bE8}JoXzYi4b7{{pI>?%)5r)ME0D5#}sIPCZ zGP|uW!3(KS{0z&5yg&7sHO03#vPS8I9JwmaoN7#!J(qOI@H}UShskV53)x)K~oOlUmiRE>@;!Ou;*|9 zb!OS}mh(@M`J3i`Vr#)&9qxA6$F8vZnJsx7D-!^KtU_@4HE(x&#fow9Vp9<(G)cBO z_g%0F1rVS|lE>V5DFm(i?UH4AAt>M4ReZ%7EK(6${>mFaP88Km6p~`wwW%PM=LDOq#-yMSj#QG-thSV6t)d z_gqJ&(L0i`f;Ejt_Mq+uU!i zp;fp7Ow(|u!Y+d&_7EW^Ay~Ooxj0A1h6*&Q4U&}v8oVlNf$1xe83=m@R5LmH$PfkU;SSCX_ZFDpN`c)I{B`~Q&p3_~xK@_}-I2CmXP?l>#CguasT&cYJx z!exh^m0w9@hxO%0u@bZ4ddlGo%;;#W<3zI>_l&K@txylgYSiFiP3|!M2fZv{94; zFkS#X-Jku6Y1wV^m#RqVPFnU1ld*F6ZvSSgW!~D?iz`E_k{yUYu4ZQ4a`hu_h2H`t zt;{Sch-Dmyqz32fwcJ$_W0|4Lr8-(qpIR%Wmgbtt(h`Wtd-*3u3+EO@T-{BZa_7z4 zFNt8-_9H{1Lt6EC>(XOAY=nktl#r2ndpp{iUcPKR9fqi9Tr?Q^Qbl>FKIex{u_^1d(g$1>a7rRsqDGjgeoPPsmFbm;<26X3%)u*s z($Y_0;sm_8oTX|6z+??(GEi6+jYqj3+z<14WE_R=po}$_N88wX7^vm|M5YT-*haI{ z?YW|yNSqal4UjK#c*Wj7OQqq4t{B%g`pb1$l8FJBQV=JjI$_9AgEX^Z+L$3cNNiX^ zUx!hc4wb+d>T~M9TnaB0z>$`ij0v;q!I?cne^T~D_!U(@>S~N2poJi@Z!6BU$&2yX zg^<#%U&x=95z3m(3a4wF1WyChbXN3>U~+zJ8P&|9tFlTX$m@tE%Ml|$mEg?rRd9tk zWdBuO17Ae=lwzp%Q4;Gqu zmh3VhffAGGIkS^}pI8u03;RH{ab}qdl#9kyz18)>_kjXwHY6Ou@LeZ{?~huhvcd$E zW~0+f>lO_cSO3sfN=d0bQbbh=F@J))fvpN`g58OmX zg}JTF@e@-KIGh)dN0fyYa|g;@Z&qde&y_P-^%cm7Z`@s*8tZH~vp%ekHd2`hMxdfs zW6;mZyV3{ALIV(ENMpceP~#C-3GK2&A?zkYA+{S%jt@a^^xle@3+x6h6Fim@NAjzq z$o;)t9UWb$occA`?<>Ws8L04DlARPARpp6x&~8JqKdl&)!^y7fC=ZpFD@da@I0{Mi zTq5z?m(Fe7K1*RIyV3cORY~en5aX|Lg20cG_dyi$;2!2+961+q-D+3u+zfPuJd*i` z=+Hp%cJgga1zDx4G#4hLTywdf1XnCwHy|6ot{zbiN}$!+;*ND=<=6p`O>fI1~tG(}W>` zMAcz?%B?rQQY6S{_!-=Z{jWGdhdlf<2lxC)Ci~52ABLzBTuq#Gnmj<(!IbwYrzIh9 zba4jZ6QN8a69s5;U)EO3I{4^BKAEIo2ZgB|Pd~H!n-*&LAhA@UCElM~lv2PUz5KzyShK^9r&7(Dr*dOyu%8fcq9nL0g@%cSH}(4V z&HJ~YI*qExC7m5_3$9caCJk2PkcAr}KG}4|xlpXm=`W037o!qDw`$=N3>!@*okY~Z0wH5$ptrlRbGsBnktJ|| ziMJj+=Z*pVNe=#Zcs7i!^ag&2`UjGVp2ee#`C(NRo`K658&o zJ{oIWcWQHISr2%A*_Mcq8iX0fq%PzdclUm6cf-=9U6jWVNTY*=$E0nhOiOhQ(#dR` zZt%bmm&fU`LY7py2L;ew5QwjI#K!ABv^l1L$DS^2~Z& zX&C6UB+wdU(MhaV@=b8-N2@a-fyIl5K_Yo0z|WnAgSPNs0To~^EkQ=Eatp8$4_h4E zvVbEXe^TuKVgaEkCWkN&RYEQt?VFqHPcA1lWh&>FjaO?tD95Y>aFJZ&s|Yq12lhQG zkOOGQ{1hSViyZE6O8B%c(2U7;2`;){o5A2*$-C2FjHwRxk3hu&HKX%fVgo%JYoM>U zXw#JCz>&@KS}Rv?-sTS#JO7daYc0z~#S$yqNqnv@)W-X|yLt?X4MkO4Y00(9qB#a( zJ4m)HZ}-?aP#i~kMs`vt1ifVVE(_v;45w5nN3mZnHvbY@;godl4EZnXEc{!2V4vrw zVa2aZ&mSHGz0#GU7f%q}A)%)j;k*F{<@inL=*>(sCBI)0o# zu4`yl8Q(lO03B!Hm%CAH{2u60z7~{&kvU)~XC@Rgl(w8t!Ie&5D96uod>7$7G=`&n zdSMblfYXC@WS}K}$x8^JmcuTN-^q6403_3xw7#Ry(f2B+MdBI~(*wA~0YELC{l!^v z`XL7nNFz67UwjN8tuQc(M-DRQJ`+BXf_!uV_JAHwrOUFkP|RLWLp|oh=y7Kl&yD8E zEu0A9Lm1q)u#nsI$@a>Lt0IIp%+=M6P?LlY?vi6Ask6N2mI6Mhr#G z4GUe_0FjyALBv~ot?jkcX4PelLym4HrMAQKL$y_%L@E6dI9y01UKQV+=`+6b4`CEK zE#?9fO*Ybg+c=LAsOFOcrm>V%1^l>JpFObr2I5-Aov6qzE|~&>>6r*lp`pRg2aKoD zBM5K(CScosCdjD9v0)cS%T1e$%<*K-i&KDt0$=?-X)*9z<3N_Y(zyPUK+mo@LoiD1 zL-dWTR~a3mk}*0m85`hNl1Pvn)jGGUr#PwELAP3J$lZjZl2Jr}RGGYiC3#vY4H}Ub z4IS5^5qpYLxW5q8cdjw>Vi?v&Wg*bb1zQoCOL0!ije;-San!_mVa@g=sMSL zSK5q`j)h~m*K%&1mV`b*tRO!iymg62hBH(m zyZbA(@!pnaUwrzkt)Dz+e&Y_*uE66%7bfjZF{tVB=imL+Pk;E;H!nMS2doa9Yh3#8 zr|W>$@GWP<9tBITYPaiJ!qO*HSEK!sKr*5w?T$+}f|e63%d(QAEvKdK@ZRgrz@UET zrt(nzDYogiiKghfu;Khfvms_UiM}o&w>7T(V+X;gu%w|K*|IrJ(sbMGJ@Pw~tH~bZmf#^tmoWmX)4pKfh&^V7`n-rn6xsjoomckkc6eSHHTy2e5v8gqS+ih#3W5Vps}tvgPo z0_rnTDtalstchWDq6zaW`0my9@lLi}nfpwyfGPRp3!iZVyWs*1ehE}cv8mCX7vFXa zR-%zFa_{Y)UXf!~jqhxLRqdB)d-~<)UwrlG368|j$j~IC5<2Hr-O%;b8V=k_vALkA z**;Y3MZeVaLuG7mkdBJXM=p^SD7;~asP3OU9Wi)&Ior~(ax71bTA$YEsUDw2A0)zq z9%FJyxH;+#sl2=CBU4reZ~MgwA6mSSQh&3=IdF{9USmPK`%6G%Malu5l9sfMz<-Eg zxU!NUe5G}R+6EQaRdvvO!ZLzYF0W`8_is&+rJ-`m0^Yn~IFjJYD{G4Nj(q2 z>u0BixBXv@Gx<_;1dh5vi^57$KaAm$Kc53q{YUYi&m zs>~COMvmf?)geh|N+U=;$Oux~a?cn8WB?>A8@9%~L_o~)859f2CEf51lqMb|>!KuV zWz-JVB5hp7?hcfUdX{EIW{Cei*gp_s_{H)zO|A<)F@by)EbN?iFZ{nW)FfyJkrSc? z7j5%}WonmfAYY6^^Rj}_aKnVWh87E^-?k9NiTaF{`t<7RzM}2+@>J*(3pN_A1UH|+ z)&*{y4YoHwdHCsf{@j1_%YXj!X7UxYtD1J1d>ijJ%l`E7 zHxIx4=9|ZlAAS4q+ZWC4LvkccOZju8D%W{{dky2z4vcFqLBvd82bxhUfGwDZQXk;E zxp{299}JV-e?svF^TmCz#o^EaI8kgWPo;I_V$)V9u&LhRGqjL{;E2LT4Rj9O?-F#~ z=Ijo(u;=TkG7;j=%`VmE*7^SZ6O=D{Q<)spNUn^H7?U0tnV4BGb|?25^Fpx%_jWYX z*DUgoXB*>~!Yr6OCeS-RHWs#OIaINfKpOUvyoPI_c9`J`r3J1VI8~9iyNeTaY~`fR zYaEXq7?)?leXz$pPFtFFc5r(0{uasMwHq$k3v}6daetNC*(u8X1#L%obAtzQcg3!3 zzkYl7?%nHm?{o%sIlUB-mSSHVQfEHBy}h`5`|dU85Dgm8LAuIvJbr?##L>y#hB#(` z&r%0|(aRDq#n4S#^tfPsXxZl3eTmZUgT-nU>&Oar0wOX38xy%v_tX))wzfY06c$aN_gcEc(WM-g!nozoeehI} zm@rj#4mQajA8Z}*Skl|#eD(BTb$XGaG$v|nQq{NmM;orVVWqkHM0I9yQ`L5X43`XX zZxddQV3yszZjM2bS?r3)Hn>A(Yh|-0uEi=ifI78YXYLZErH6B=!q_ejr?`6|Szq|N zfk=oNn+pT1*g*v{_Bp5u`YdKX&u>56zJARS{K5!st{X2U0HQUDiH3&sU%7sB^X~0k z2vH}Ow|8%E-@bqI;m7W~M#}ziar#yR`SOSnoPb{L-e|#o%r$iyL4r^aN^?{T)vqe*e2a{Pan0ge0~?9)RPn z)cgAf$xOXWkBrXI_Tx^A#yn1VYI}DE_&NJ|;R8e6H7jmDs zPp_P)*<>BV-QU@!VI$ILIj0o#1JYl0uDL$C;0m~Fzd^4Aen$8rbhwDnN#f!e46#`r zx_N9~ZePISZoP%${naVSnr6wFFn|gFC!&Qhw8Zz>kidB&^amFTKDciv8_{2@{6zba z+n=5h4Y;<2dWF`YPGBajh|fUsf|^KLBqqF6#6a1<-cN|oiOrZ{mkq0FpOw<|CG#Cw zqu5y#d|9tY*f}^mYdpU%lmDyhw>NLz-8sQD<5H@Zb?s7paxOeP)yo{gAtQa6G6)IQ zun^%S^ktIZ9nHfd17r(VeTkE6Qv*Sv@{NLXSH?%i0Y~e*$+fW}wImU3h_#|y93O1H zE;YvH7u_Ge7ObbZnjUOve(~(ieI5|I=Uj&aeL9VN2gwwZK;{#+OGY#=BZx zy!htH%U3U+J%3)j{I^D3faLwAa*;!DATVxF_r3HmgF!=lY~F6x-@rt9e_6ewD?Mg< zJ==zEB!(%D6TA&6UA?~ZCXO>Y6zD}1^>vIV*c^CxpWjB1SdAixgASmh0eJ!4+p9)P zws@W8+USzY7C0Imb!vQ^Jd(yOqe`1x#WI7O-`)Pw^S^lUys39AqO{H@rBRqEy41i= zRG)nU=&rb@-BRx9<(`3!Ryp92!B7&tw}q8A!jELtYXhad>WVr6+*X|UKZCA-yt zDfVVU=J0K_2o~Vm!T9hb&!zmu^*yan+1|U??>|5QKmey^N)4Xz8Q_p)(0N%h0QaZl z<;L+5r!oL(AcrFKKv&Dqh|;{aA(_C4d3_UI@)#N&U)!LjW|J+^aF#fkY{ZNeb?I}- zJBgRSmGEL3uo^Edu#JzZ50C?+D%tk*;TKoZ8iTiZ-c~GzbU{xetY*=v}mHj(0KkfIlAq!XIIBu zkz^mt#gm^LY#HpQwuS7AN)#kh70T`Cq{^>K{D9%7p`om17i&QWmK%N+|6s{^5O*ts z0V|4j;9mkYh3L!IvU)Pss@8A_o^=A@w@zw#Pk z6DHBaFJS=!2FyJe_3py>=16zjlPAw*gkOI7#rMDay&rtl(l=WDr1rBZ1~oI%*WUX4 zRa-~P%a^ZSHNASlRyukHD%$Cbay^NP{uBdnqovp`o?|bJ9zjw{FOXqEJ}3x=bJPK% zqH%%ybf4?3dOlCtqjcfJV*!G^f(>e72%LY^C>P+)k~bt1fF1)LQu<{ln=I2l*ew=d zxHl!5$Jm^&Os?(-EV>~c#=Vu8%InR4G&eP|fF2V<#v!Xhc5$IHGb(wdS#|}(k=(mH zp|X-~@yAqz5l~nz7S7_20}wi42nZybdvhRuH;=?9R#$#$x#6vrVhtpoSw(giUIkl& zH_EK##EMyvM@HaPE(c^2?|}$9;fh5rLd!&!5hU}_1l6{tR^1%K`b7V;??3Jx8m?>r z&@6R8HWH?3(DjplA@o=!XGT9}Dl(by3=YfPx0iHw$E%4m&;p-Z1HH}9n@-f!n^41x z$eU|Z_f=Kf#Q4}yXGceCQ)f?K4+ZHLEw3K^=!+*UBUr@X~*aekXc-_F)HjsMj$YPJ#-op-Zt$YJN06rJPDYRxJ)M*^j+Cr-8;Tw zIOwD*T!B3#dEqiCo>nS~k>H({LHHKxLe7{j&Hoi~EdY=$D5sJ*JN&|e1~-+{S&{_A zLx2kh@{!CYW9L&{R$?qV>yx9y@|q!Tw6#0!Ui}`060y7)MX!xZE9476d&#N}AZ<&S zM$7By*E?C8qX8FLNc8Rk{K2_!fn~LoP>>GEzOm7rhHD{X0gh$v8t`O>IW@0i4dX=< z7$JbCJlK=j{awBvE%WBqUkJ~-!pHgaB&uO`z&W+GHM&=}Ya`8HHCs2mctC}Hebst} zXeF<(g6@&2>B_JPS-p~}c@w{URR(V~zSkrs+UaQ~NOKmJ?A&x8oTt)f#^wl~Wp|6f z2SF^&(SgDKuC{Kn;e$PG&27z3zWC}{8)L(FH+c!N(=@3Njv^1)Pf&1haA%3b!ut-`n*;iu~g6TC) z>axSkIx zPz#yB=5ac`$#g>I^g=VjDnW5*05SIVV8O)KP2)yvX77xe6k6$Cm{2fG?lXIA82 z1>#7}mMbC@{I@)vb+$?Bk`5t{hyuDR-90Ghk6#R$;>`0K*<*@=2bQW;(FaHd~ zdeGkuYNm$_W(`iYW}=XqbFPT@X+z^bDXf_o&bb zKk(}At}qICak48-*P#s3ElPsOc2^C0hUXoz7s`tub%H z)eAHllIvr1K{X=&wCp+Ar^T`tCsFHODX2R69$2CT*6s9Ks27vJtIs|*VsilGWX!x= zFP-uBmRHRk12y_|Fde4otHMqerip_Q8k(q7#)oVopd32wFUpX`^xGPJ8r@3KM?}aJ z)agn2XJw>|HP04OkenUT13@W{|0Uvd#urrwWum|3-3INiT6%jsI;(5C`24wFT%fQe#0P~7x6Rm_!Cnu)y(5p5?gXh{ zDV^xCxREapeEHB@LAwpJ7y(@l-)5`f} zas4w*Tw&_a7fr|jpxK>}dFX&#N$&X(jtV;Jqy^Hn@i{l*H?#MVl+^t;6sMgXP&M=l z2{?xr7zC5`_?&0uInqag+bgGG;fvdWc&G|Oov zrPGgG*gCqoO4G`lhz%%AAbw)oDWaTX^+PeZQru|G?oknu5lqldO&>+sT_T||f(!Vc z3sCw2$+*tyRdIPAOz7l{FT67%2R*b+K=TOXm|Pk57om!EGCZVvyQz*0cK6inYt{R+ zIYGfd-%-(POo5hLhNOTp{Hxj!wLB^go6GBXF-mHf)Ae~Nw)N2p*b0r9zeyeu0}&T6 zTxq8|)ITshIy%S^$e!VV%{>zfVL@~AndYE^A*;;hiDBEQMl3EJ!6uhfktbOCAZXE) z3X&e`gu)7k_vXeCB>f?xOCj8JA^8j0mbiCa-2S?pF6lqG8&g3y4P^ljH=`-A?_iyD zfYkz*L4yjPUZ3h{yHXKEv=@pSBolOcb{(w2=eT`gZ=5xz zw{K0E8Sash@yRUG3=vqUqjtM7g^$V)6jQ&0W$Ya)3y%)8-lIb^ziAsiM-#eZ5PeohKHJgvv=n4FbPMdC>a}L#5N>Bcmf|FwsLu8;v2&*HHC`#$G=9`q8u2fzkQBgXP((-@*M;OMzcS z^u>tz#E3#s2D`H{KQcNww~gsVW?jIt-At(==J=$n@QJ2GRd%lx0NycM#l6<*jg$AR zV0472-Q;9tVr0l2v0yCHU%7LFvUrKGLeCThZ?0kkui``>SI&fJ%}#lf@sSaec$)P% zTS{q-Hd;Eh5J%DPvN;e}O>0K&N_4-iy}hNar*Ax!7sf%&F_Q-Qi^-i6pusA_P^xY% zATlVv<$QG%T{hkP#X23owqZZoq8%M4%llrtrE=*&P8tgF2mUc5O+Yc9nZ+BL*%CL@ zw_^DmB?y$~+@lg+4p&R0=iGWZZYXI|*5cqN5#C;2H@Jr>7x0EMJnb@sObHFm)94XV zT^*GkYbHU>XJ&1lSYiW5yWxAQVL@S^IHdJqz`oI(x^WxuKuG+_|oOq2+%Yz zv?X3cfA_W=sM$5BTe*+jbOpW>5x{D_?ZvYfFWUwuR*uh1(Tw}{G*Ic{CuvFaTS#Xy9lqQ#jXbw91wLCw??V?#w{R61 zzrVW!Csp%iKP5nMIS(H#p}wA?J~b{bxR-xxh3?#74>^q zG`>Dw9hH+>>vO^u?ZcIoqZ6E_u?pu8pp7gZro(SBoQm6TZ4O{*y;`Z0);!$xZE}?a zb&c$#>qkn&aslzo@Ziv(mf+$FrSI7)dnzh_IGJ!T%FB;IpZe6!+3d%FKxU4D<%EWa zKMZV{nVuL)4ytdLnO;%a(px>%$uaorLJ0=>;7u>p+94U^+o*cU*Z0J|>RE5L?wD_k*YSCrr?# zzhB}jnvZszmlYUiv$T{@>1CHtnBgD&JgTOWnT@U5Xq8S@%5j9ZGi6(9cFFxOAO}+4 z#$tDityvFmoaD%5v2oLsk!`JEy+iKsv%uk6YJi?{? z?`U(O7T~J4v%9^$wYAeK(3dY;JME+JeiSg}Dh&1e+<1TUvnP*Vwg6{Mx~JpAV(ECI zv(ncR@sTUp0JpWjC{%-X$eV5717wkn^h0o@u5MqywRq<8l7C%9h)TnNa|dCS9gtKQ z+7KTX1msOb2LWj_Sw8JXgSx&H=PCX1214bPPCIq&;geFVAQKbHS|SOiAvA?IHh&(# zm>1_maKSiD%1JVAne+}q@3g*_iNncBg`R`oHQj+r;&jZ*hq{4El71gae9q7^z5hw; zzdA8ty>{$vNoqFMKokm$=v`#g1|$|!(BGCg zXjuOPIY;<{RBA3XqXOpSE91#)>~Yvw9kBs`);=snnn~l(vO!wuXut4EJ-WN zxF{u2$F}*>rW>N+$->ATMDN)#K{UKyUFG#~pq!l*LhBc+M3YM+@M=vmS9p+FE0U4Y z_YN29i}dMXK_^om2V4ujD-Z>%2la%BCM)KFVC2DTDJvA4f7xe1c&YLpT!gdeSi>{n zUAGU?q4+OGASToBacscp-O^@`4jcEU177f41#Y~Cq!SberytZG%r*Rbg-4ZSWXdTZ zHpvC4w*;PcrwliLN=wYri1q1_*6f4-{x{7%oy|?JG;Uu$ef;G4^XB%RzTpvIjh;) zR5?ANkA}bfciXDMsj9+Vbt4#-gEMeZL`~>6RMFXbRXl12!6ZrQZ(qtb@IMALIB~)o z^kXepT-w|N&gNoluDWjlH{84IPJL^AqWPh0<2#PXiT9iWIUN&qRi>d6Wx$mv=NkCh zDgh)=k^Fc_dwBhBB@H%&El-7|B3ApOhA|T|vJM1n$otqjxjCRHEx8?aapMCbIxu>IOzPBR&m7L&NOkjX=h1qCgcCr+dm#l;12)=&aF)%PrX%L_R zmBD9$B*YE4L zf)3PQU|8GRZe05lZ6=wR#=UfiGvrb=JMm_Ljy{vLts9_1XFF4M+Mma_@9zW)?k^ac zHe?F&(~Il(?=lS}`^{)9eG5rNCP)L6aNAJ4yJ#suW56LkM7*Gi71D^1fR&|~3l|tC z%gKGRzQ{$>IM>tXH3Yza%1-!;Ouc7y9Lx;uF^y4+S>*1oukLgWuCLz$B&5UqVRBAB z8m6z@1z<#^n~#|tQzCiQJ}cG4&aNpQ-zE=ph-m~0K;KlhWmFQ_5~k^*WT4ntRl&IE z(Sett)g}k~9`rrvY53`6J?JNESI;b4%u+F8o#{qlCb1~G<_Bh^_Sa^HI-8z;_3&AH ze|ORPvyRs0)|XG8y?oKqE)oQaovfBSx~5aB+Zm#z|G1^Me{6>J(uP~0m=*x9*!-gs znLUQA@bIvJSb%li$aQU^t6{VQ`)R(Rw4gc+fcI_Ec>}$$$6$7GaJB$jTXiM&NYeuim{eUKSZf zfZruYl61|#hoq>qj{Pm5KimR;M<4k#%GNn1MvyLHv1Kp0{+j(4esbg5J-?0Q;nikR zH(^{vVDqjeoGT`9aC&{`lTh%&vAi{6e0h0y`z~M(OxA(;!=dH`ZQ2kg4$^km-JTm8 zvF*o7J2xq2X9y^9FD5`sR}o_#fJ@1sdxAxH&s*6XrRx=Zhz)87V9id9_V+&Mf6&`- zZ#fU~6S*>5E;W>cZE&YS6!`_SP0LaES)Nuh^mKJ}4USLEjE@ZV4fund?VVjVxr+j$ zLc!#Op{DWSp5Fexp3c_h7mpsj>K+~&LfJ@Tm>JY7^zhK+vZ(|X&c|>51_O_hujnv>%*ou%m?zx7y4+u%FBbPg|E*SFQ;<_XAY%xNYJ(+H z-#g4YU7K7at`i+$UYlVHN}hQFsVlW)fXGUz+Fe}!BLq53*DDU-;%IB{?)~*SQ@|5U zqs8w*?h#Z>Sz}sLBCG5?gaW7Xlta*KQtnJCeub*`cfqr0h0=ArFeb!*|=-@B)HT-OJ)B@%? zZi(|Ttq>qWzmAtF@ltd7^Q*hcCNfE?3HzQ0ZHkj0_ZI_i`@1@|<(1_VlM@n5Il6|$ zSjMJ@^>tfcyl5YswAv~3&FrGb1h9k2efi({7nMxuMHxaU1oCxHmGN8Bc|r7p?Fn0U zG;5Kg!2*}%ZVP6uPu{#X2&x%o#Ky_XalJqhPp2G(F5yxtAHt#A{YfTbIg$it9c67u ziH40gzK|r5dvOct%V>bt(=UcKHPZ5%dD|-w{EP+HMmQ0aFLCbt@I!*e-TL>uu zKnr|`1;{Z)1P(ZEjIUS1%k2VztORUPhx74)_7;CzCcm&yXI+7FXx*#qB2ajHb41iQ zOjGR78KgT2SiYPI>o_?&D5UUIt4Bo~Is1B&^z#Bq0`O?hE??_@ zynXlf^~K)dn;)B3*FX~ZqplHhS$w3QTSfvEl3!OVl;VbvL^WNQMyc>)1P7dkF#I=n z%*Xjd$+NigP;Df5dW?tRl*wO^O*RjWhu~gi_8jvFP3PoXL5y~N8>YUbWA9L~EOzda z)EdKM#V02l)1(l3VZGB7=)zMzcI+y*>$^9vFRwqj*kc440l0k)UNHzW+}+tdR1bB| zR&|xCJoOO2YoR*vVC=ytHfd$N--6z8ofB*$tMZDyQtL%7lqtu;LgI|UaNctyx=1@T zuI}a5VgrB^N!eukgl46kTNC4xMePE3pfwZ4)AnXk7p!JtD!oWw!BHbquPrcKS z6as7SZO{ATHEC;B@hr~4`b-`I#8UACl2TI4WY&rbI&<1RyOxfVLrCesuj?P_3Qe!? z>GVl=XyIs(p#K22oC*-3Sh$!PZGH63w=YNWm&S)i>51%+whT^da9bwCL9Z?ap1Mu@#3Tuy;8z@DPz9mWlf_`RHkb~M%@qq^ zh(VE$z&hXu3}clJjLKcib|BSEQZM30+W`pDz@!u}pT*+S-4i>D%WtkCt*7NEtc2EcryP?crfo*N`-5T(h*t zXV2D)iIoUdQ9H4SnW~R6y_oqw>ky=I9f_enA**+Apl@i>#9m0dE-76j0oFGySJhyO z@Q#G3w1tqn0;DUTmgRP6$sne(JNt%AOaru09vh(S;EWp-=@u3m&Cc8MGh$6xzKakk~qZhGA{S-Z1 z5#Yo;&F$9R)h{dnh>tw4xhoDFH=eAWUEZNqpB^Xo0`3cH#m?`h0%3lEefP8yc@imq ze7q;||F7&tyLx?VLKgN>Dile~GboE;yR4VF83rC7pMCL3I+^gsXXd!Ijk+1m2t%jaFg2-T3h zz78`NFP=R3_Q4kozs0NuP2txq;7CM^hPpj&F#^$zuQW{&4Hb@cNvOxT*ITk@FW7sf z7`~5?A@nr3$4sRJY7j%lMu!J_#DWei*pC*F@g*|ZFiJhJVCuP`(Go?33C!&(pqYu{ zEP&lEt>O!C>@fjOAO*E8QM{mb62|G3Tz~~^t^Vf{z=rfJC1+ju=NcacBAU+w0-K`3eDnft zI^!)#6vf%!a;$EuF)0`(1XaQiZ?Ee0fEqDKZq z4HtyGlvz@UA!AU+>*+uWRXzcjq&TxOYXI!%;OKBq>#NS1c$jTG;0>ma@yY&{et7`B)rx&g%l{~BFuG_m@y?FmE#<}oGXxKPAb&VB zIW(zGT9_K|d1=tPj|y>1*HD)e=}lGN*7W3u-+S<#2Ycmoi$XTIm2gMf ziT6rW+hbG~s2-kPky1@_1mlUx$A94_gzg_%lr%(=b1@P)5VuZH#jbGQwxxSeORu`H zTJqL$4&;QMFuWVf0>c`fu1u*47O7iVP;HV%rN4%}>qo+Y=cvgew92w#lCUI4=YO`7 znjM&0lSWAk;NK#vx){uMP1OZDAfb}gJ2=ay#OZ83sR&Mk)oE@)ba7g=LNaCpO_dXD zy#-Q(8zmI!TJPtFo!pwF{qW() zM+U*#I6u~Ug?5V=XC{I}i=46;aCV}+O3*NARodiAG5ve>a2p`-*&pNvu?1p^OdWE9txBco z$V5EP3Xb(`p36sa)4&}tZJd(@=?njvJ~(rKnJZ1&#{OBz2CEDm}llmiu!0%-|)q zQE~n6$Xdw#jaw16Uzr&j7++OiUZkD#i*IE8upJS1=i!g|7z$qCe&;St7Z2D#SnFPq zg|2;FSQJZ4HGqU9P&tyW5HrBimho@x7pK=!kj;gO zQnl{Rj?VU1U;pUArw_i+1%Klj~lpZ8(qU|&c6fL?3k+|PxJ zoDGn`_9b4!>&k30lnm*O&s4d_?WJ!fg$W#($(XST%pRoCp`o-9{HLO$8+pr|3ArH# zhec`qyruUYJwp>#-+~8V1fn-cJGr1{BVK#F{Yutn%1+rH{ZeTj>*j|7*sR-nrYmBK z5&u}lkEM=LtbhUz!3d=GQk-}Z=f1lBhyH}q%Y>|9Kgo#^Z>t3(oGDLB(=W_x@`|Bz zu46nbnRG>?zg9kKA)GQ?6k4HM$n+iWl$YKV(RKZF*))S#!z{td|bY z*9@J+ebivH21sI7ArJF0U8J_nRh#R4vR*AWadL-41ona0d3~IQ~U| z0Bnl^`zuEWdRrd-(vKR0&wLbpR$nN)k@bFce<=vE#*3{Nj1Laxvl?!P&LWQXE zNLEe($Hu>V7PIM}jH zd|*Jnz(F%HgJhzkG(^0n6J-DKZU7ePtzYrB4Nc5yi1-;IZFn~rm)rd9)hU5&(7zHo zF5TZ`i<#umedwn1q;(twQA7D^*Z=v1x z(aqh5_aELFt&*W}2d_yfVAQdy%qBvs_K0v4H~&xaL%!pR+y-GCt$C!*#X2sIh3LJV zZH8gmpr%dDFWO%H;Cny({BhUB93X+tvz%AWj4>l9p_5V{SX@1Q`$i7JgISc4nnh7s zVkr<7*%k|+s5`g5e{^|WoX^U76NhcRvtKz}o3zuG=PN8wXY-J5%5oid2P1jhNsJC7 z-VrB@1@yH){iUxOexj>AJ=FI2t0xbC`1!*pZ9P>+iB>F`;2EgT(-YxA|Xy3|IaJCl)r7cp&#+0sh)wnVH|( zzqp|f_{-i%JTg?1eV~R^qCh!Gxv4aw( zYdT9RYkVwvGfTCJA=^_j{8e&_in;swxpcMGSCj3?&N5&PSJXX^w7YMc2Z*!QBn!F( zgil*c+MLXiYlWn!G94q7c1RkOQ20%(lXOY z(@0FC|C$v`3LuEnt_9|X-+x!`NEKI>=2GFPWNb|4FfMgT^T5x8Oq5bcT5|as8iU7?k1fuo?WdAB>@<3}>+H)a zL^f*$;`z;ur199~{oM@+_extq7OAg-qv7{iy!P(bC~w@p5g?$LlHqZfiCADDa&)M> z>1h*!-SF_}@K9&F&2j4VOghoU+dW3qzym0wtnGt}mCe@6NfpGwYRFDB< z(+_a&(hAeFPIoEUUI3dq!k!c60#N$#;gJ^!DeDyN{!z|9qa8CDvfHtMV{BOdDHN`oH2qk)NU!9Oq{C@s1|dJ)*s+quQ^~=+MyPMAEghcxnU#_%8E^y zlu5#v_MZ@69R4jClw6CnqXIBevBm^0U_j-hT>3geL}9=uTdTA9&+Eh?$n}5*LGM>@ z?sOCES*673d6+2K~FS~=hj0_F5w>u)po+|>s-Jv<{NMkhpprBy&f6)|mnW}v!3b2X$b78w<>NP!cwRkWl#$opB6$%Cod zd^F=I*8fs0zyZYqyc0&CbdoWvQC$!PHl`IJ)@jrB9B>VsUjyV#_C08R@S@>Afe%KS zo_+S+@BGTApFRBM$un{-FJ84aH@Ej159gf&;o}Fwfm6A=795EmgA^#;H3``tRN|!|D>4lWa!4u8<%vVV%*d$dB|UjD>&gFbcylgxd{$}&z-56K%APJ9wpOT ztIo|$_P4flcAFv(OUy8sy_|dE11FIeq3{ki$EEY&yro4eCyD1k)nfgOA>yNKQ`rS# z2)s;%2S7)YA#h-Py4}r3U7OiNPS}5*kh|p*43@4-r7Al z>{qP=G{d485#t)yeylG@dT6S?%A3odTq}-7As}i7zU)?fGbSGmHS2aIB2aj1;a{q zAdV<2^{9z=5$5Gvc)Z1sdnad$<%{gW%KiXMP$U<@U}cJ)ir}cvG0?A^#5E|E?yYFc zGLMfPV7fjhVAN}cdPZG9GF+a4uu>{!6EzwU!3uO4m1k7Ls{tlJoWZ4v&N9WdirE$__~9xqqm~P65Y?!IWXP9Z7zf7Y06Zp_z4hUthb6>C2S4Zy?WtaaZ*qdYiDN7Seu zN6sMtZTSNwa@0Kod8W8n+dd+C=69s8uSO{S{b_Ml1-~?&Pyo2hXeUKGst@3%bTB~r zI_01P|C`bG4HFw7gl&1lnXkVtE>`qq?{47((c-vp{KfgbOz`6DVAXKd#(W%Ijvt{x zl-I{}m6m43-iT)Pw?k(;x_f)NyLu@k7|c(v z?ZyeLt-Y#lM44PQx#(P=6RT*`q2$^&OauD*zA8*BE$$f`rrfm2;02ADbIo(JZ4s=R zs&+p8*nj)afBe{mi&rkUUs}CFqNPXQz)lsKofBk)46lw&O;&x6_VKDvJ1WrKHbBdy zpi51UW}TdOzsb=IzK~c)IuIiQ#{>zN#*P)$qjJ1MG^vpfOFANT=39s-8WxNESpW5o z4#NyLu3jsyzjWi;#V4P5?C~d`diK(lYetE$Ub%{lhOduIZTB*BBWDq}3#-#xsTt|5 zZWNid^jmbMf%5%sqb36)6hH58>M#`>52bM_GqmnS5OpNSRhF9AOi(7n@mxQo&zMW_ zynr#FCX!cTV3c)%mkUc$Z2)<-I%QZZS9=VgF-1oKu@5_AY+`mwLkBQ3Zk=S^R~zi? zG+8~^eeKGDdDx4cJ^kGligu8ZAR2G%ExL>8H95kd4MX^|3+*dk)?kYMfb+h)b8_e8 z7`X{AOz`8?;gY)`@a)r*F~mPPzq<$9beZi)LLHM_HZ^&KsSPvlc!36J28y0>d=tAb z>O-A9VP&oW z5=CB$>w9o`hX4*5-bbJ%OFl@Q?PVt8Xm@SF1dVjO4zl%z zMWZ@V>m6*`QoY@_ALdT&=2L(6hySND@Z>YkP`WW6!2I1keFOLa4I&bzc8Z1_#F^mW zU#E(h-ONpvl751JBG~c*A2DovPOX+jntFNK=nGZxVPU?Qs!f_P9H|#$mL3(&*(q@) z?JZ1@>`$^~$H45TkVRPD)W(q69Ux|xxfRJ^M`w3;AD+5x$%DfNU6e*fr+4;_$ZZ@R zfoueAjxxzTZ(M9Hs2uqPC#^n&d7d4s0=NONMV(I`F+?j3qDW|L=p$qhGfrhkWx&Zw z^xeed4hjm1rQ5DuY21Z>RCSQ;Q{6xN8r5{E5EW~L{G zkW_GBXpCiA+%OJ^@Y&8@qr=^@fzCcGz#2kRiA>1jIZlCWaoNa&H+XagG8WL--@E<% zy}P#PY1r%*JChm;2S>+;NBhS~OpDEZn76N`JCQ~?L|X`q%J}XHg1R%X$eq=PJ?H-M zpff09wch?oL&h6zgLYA@7_J;tvD~D{ff(R&j@Ns-x;rjkxZGO|KST~UkT_CLa^rP0 z_qB!My=%OSk-^Zle0t4!n~pP3X>j9apQ3QwO-N5h!J-PB(AcjVV(sbayms-bJji@# zd1~g(sUnXVNs}pYj#n=|_gu%7ix;onyl&6ZE4}@g$-QKyuU{iGe)Wd7w<|>~07ll^ z+ua?Wllm+^8z?tp@kHR=EWoy}P_sSbzgRj8Sq-bpG6bkb$69#F{k8@a`Dhi2hb5yt76j60R9kZgfr3Q&a;%}OVZ1hG zbRZBvLQ&q%okK;jCg8|~}qz0q;;dZmyEs&j;Z2?Hq)L-<57 z<*(wu%@|pN>v0G~<{POhfCUnSv?Sk_Y`f*m7(b?PK6ZO-5K~X@R`wL86e%+ zrlf>h7`l1wddJmEb_#Uh$aUSM6hdkPxF8ZpPo{*bo7at;i^^8LR*LF!Wq20dDnvi& zv@CFuM3CG`LRTPzyH{~c)(OlnZ>51F0m&$x-9aPLrIfDLo{k0C47GV{uYKolvAck6 zsnaKLj2eSx@bGG+63KK^=xSyICIn;)?(pKu^vkK+*bqm`_MLn_*oP-&$De7lg;dPd z!Ga&$8ZljD`!@a_hnliwN;)yKB->1cas-RD7c*U?9T|O_tj{6kafofQku@b!Td)MC zU=*Mf0y~G&(*r@ku*1Eb=Uu$HKIG4(KNP;fU7J8h|)fb`gX zdve$Ez!Db35u0=Zb* zU5hnOB#MNfmY24lf5{ohUL}a&nAtG-$n@0G^7MdaoRk!5UFRz~KTYbU@mZmu)%HdT z`7Dmj&QmFLW>^7=vZ<<($kAzifwAPps=>)jeasE37ALZFrRtt1xA-)%Gd)6?B^q~{ zRtN)@=%F>{`XhXasoukZq%S8tqJ|H2nI=RKpa&P62LVQyX`g&JQQa2R-Xbq%RS|F@tIF0WmoB zuVY5s9Pv&XIg=o&XmBpn%|ZS-83F_}a-kq@Q`i+aHN#*cRU#VJ+`Px#q&!m7^z?KK zTig_Mcn4Ax*q*DkG%phk{NyQ84W0c$I$30GK`&FITuPtOW%Z6GiY~_P{p0BK6Q9Mn z8Lic)D5>iU(7J@rdVIAJ7-IIIrs){b<8#$>wYHaYj1@=a0APrmy6q5r3o=6wZpdA4ziaXv!o=zUsjp{5@oJ{grfRT)sQm8XQp`z)mBO85IAU4wSnP@Q6mvO zm1G?F)NZe^Iy$CXK^w)4-flS&VAyVD(IIGX+VciZEo|^e%dnB`rc4Av#{^|9r_x+f z&7-kddD{2B7oTd8M#7e83D1J8=6rNhMn|VwYtbb(Zn&~cl0m-`0hqjY=-r^6RCi@N zC6A_MZ<4U-8UfI}2EL?!ot%tRJ(2OW+oL%Et7Lip>%LH6Bmlv6M-H&IlvT2uW<{|H zw8T{0wzee!#~G7PPSBu&6V^mkfATte3BTt8d`mX)4&t>FM>`$ z3#ox0s0>eOA~><$I>il8JE?fwp@D$a&6RnYG$cBy8S2G#*{ACW>1bwnnOx z7N-g$Z&P?*+>(bX%2zjLcom*N_LFuNd>IZVjayincJG6N=HSCatHygR3eWglE8Q_d zKBJPF&>7RpC;&j%%o;Jyz{ZNb>zEFiV#?xYV#G8O+gCp-ma&?9I|b*^Gz4EAzk2^j z(L>HjW7mC}%-G&Ya}vt$;c3-CUBg#2-xCd<>B)0b=ce1ngE*pQ;514rjV$syvZ0hj zLD^#b@o|sH@Eq?@Rop&{=Ti(WXg#Uupj^s(loHjEY4Q=`|6g~ky^~0g={GvsW?ezX zw0w9H;n8ZgkfbdYBg9iW0qI4izZj_m7rkxM=4@9f@m%C(Fl_u{_S1P;yE@9uNd|@5 z3Ut+Dz&dh5HOFS@R$uMt8JkH@Kw{*Jt*s)2tN}+!Tk1unv?(T+ozdXkx?!`52~MXX zRmM!1sump|H)k&~xz;%V!Gj5L#TdWX^A!po<*d8T$D|=OARw=h)n=AjqR!w5ioY$- zYKVf~l%-}8{oM1pxng>sGaKbSz7) ze<({yWcDPdv{UlYJv13st*9P2P8I$UNdy{WphbF;rjz$hn_*oxMjcuZE5<`CAW!YD z`Yd=|1H&JvwS615kmgyYq#Q9ZNQnU;$6_jJJPGfb7EU#&WO9651^xwtvtH*}P#R&2eVSR0`K!LUUqh0K2X@Q228hwIzjDmL))IAmX zxc5)9L4o9GlCLjXKXgLmO-?LEZ#WifMk%NTF`bWKPXOV$QLCZ{CR+q%aX?GL->pLl z(gwXmN^lkv1s*M#iN;mZosBdepel7A zPA1nyjO>F+7#D+BRE5qNrgbx_9{a#V8~Wdtu8DW1^n#Ze8mwtbe~ZjY>XxE0Jmnh} ztrrppDSTeJ(JPt3_BHZhp;DRDpe|>RWVXTi>#M7N>G<;`#rkd8Lnib+G~? zvq!^vY@n;B(p=KY+utj0VK?FX?RbFE1p`-?I~4OyGzZfCc5XBti^i8kHs#&o?1*tW z(ixR5W78;9Q3v77q`K*Kq-RFLOfS@axS4V=I*~4qT$`di0X#q$ZsZMSJ_$NqrE#9p z)R*yht!6hWDU+t7CbJ|58K#?TIG4jHrytL#h2qc#aczoQ+g9RgjXMsIDAj@=nDZgT zj$&~?URp#^vWjP1o3r%&T`LNu8Pjy)9R&y22{FQ}p+EY8@wSGB zgGtJCQB78~FE)g z`oi8x!^rnuq?tc0dCkxk?@_NQQ9=giOu`9mLc!n(N-?b;ph&c4RF~9zGW2a+hos-j zo~9=P`7LSvlTk>3S#rovOR@ECJ5?XwdisUd+ZAzvaM z@Cqpr;YXyV`g3b5%?KCT=(4L80UB?{($NAOFTG#hjJc__ zUTx1kMug0*jAM`o#x{;^^)WB!QwbOkN;sBTu45#V4k1=c!5Zq({w0>vPyo$!=9wo*R=u*lbP%(CqFUZHxFNe((Re zCPkmjCTAdHnIX-1Bck!f(+uE`&77YbEuFwH;f!MC>mh~Y@q=2R6Omj`J;voqLU~j{ z!P~7bH+d(7rXaMQ(aa;Hph_-!TD6AUKJEQchSYq9wgU(=F|400$k zQ5!bsP(@FO6I@#*c$|Vl=HqXdwH<~9Kj44@BDq&!m-Ip~1~99Htan@EC#5i4s6fBE zY_XqtSlapA;yK&v+UioT==Bu~6JY$z_xj|Uw~CF=fLx`Z!f^hkvo5mC&yEd4_N`RZ z2CqQ3Iyu>m|CWr4(fF4WLqY@c)Hct<=}_z~ir09g$(j2v5!+Yg&Az(VJ-q>Y zBbCwCd|u2Ew~-L2GmZLf;9;IO=kS4vih`uLmd`9_#Yy;c%%owe9^s2Y=!tT1K53m= z8h~h2wi=vb*{@XDs!5l_F$?)C;-umuPa-pSh#fv1WKM~S58e4VeN={wY2y{Z5HB6-kQ9v?#sE_xxu#Aa%IcQ2v;^I zn00=5cUahfc2H%GXG&3No^)%L#BW7ellH(!G5!-thB8>F0DyJ;oQ`{PMc330+!IJT7dsxOB{{R`Vp`uCAcBMnTkS=eMn1i%?w3 zrIBLdJt&uy3J1%3ngtNhIUB5O=)wHL`0!|JQNq}o_n?<#7C8hNX)&Atd}b(jR!Y4F zy*=9Af%Th3v7HE15iZNd(v0)&>x!^Z8enEKKNIUAyst%oTp_sd?Ij~(+=;juq`35o zdYH;J7>8FE{7`cUnoo|H+Jsd}8$uC{wLLe(#n`es1ZHG_iHd>EC>P9C(~O)b8uRiG zBp8>55g?w1^5K<$p?ce%#}tq0Yk1hQwPY`NiS(9*N~{@{DQnd)u*Uq1L2@(2vPlIq z#ElbD6}SQpt@Eeoben{n*~MFjM;j(8p?iqO&J3ivU$I5~)fqs%T(3C-D;qjqU(e=T zCpK^75XeO5LM2le%y>)wG=)<@o0c&|EwG-zVIpYv&F*4l47|$6wDA$ zrPN$I8f5g$d>-l$3b?Ww%^{d4I@TAZ8r@^{^oX72^|qzZtJH0I)+NplrpQFj>Pq{1 zy-W+2md@MMquO17z)HTYGilTXh5k-0NPOdAy9WTA5|2zqOpjH2x?qgep68xJih=dR zd(FUfMVm@&+0@gX0GS^$PX{Px@i8yir1fWvG zK`r4*aQCQO#X_v^T2SpO?-0ymRFR+ltbb3E=qASjcvmqSz!0-%=zs^7BQ#R zggW;Kh7Bx#Vy;Md5re?Icvfzff)>#YwiK`@Gkv0a{9dM>eFJv~^yL{B^zM8!gBF2# zbbrm|n56gCk*zgS2Uv9}D9i@{32ZLA9~x=nVtj_LQG z>pj=gRx0cJm=U|;TOlR#COLZs)_H3gnBj}9t@h1d2()&?rd=S5$-3xTMHjcb5A;k6 zS|TZySYhwB8fa28OblJU(x*994@I~>I^1QJz`k%Gy4U3H;(3sN?A~%97$|_PHVVB~ z;$=5Y`s7mAnfAG%1p*+qxgR;vau$z?*wS6O zt=wK+uG*lW~L!FTuj#BWMVR zP557GS#4!Xj3I&$@sPy$GA!g;D(h&rqx$xDJbP6j)LrHOI5rU_i)1oAsm%%E@g>t| z+*n_nDC(Spy)nKM@qzxf{m@ZMl7y;B;h!2vwk&?&X$ngswpbQz zl`rJZYH7*<@>y^~MyiA+*&gLZ9_@*t?s-KdMvR#M1WONkUW1{!Ki~G^Q$oAB0PO&LYI0Lm{C8 zqLZrfFI@-wFKyd7#)aec!mn*J$~{4E4Ok9Z6t%bK&gxM zK`el3?lm`XnoDaez)iKW9Kntzh+_qbi4dvL$c(^o0>qHiPep4Tjzy(xS~Wk?KXC10 z2S9$l>{q9yxKJ#=R%A zpCVTa7Ua8m#_B5fgFJkqg@>(j0P5Fj?dnhjz zX5R_0=d5RF(LVq|`Bo_BaLU)Nr{6tZ2It_NDmP$ z=t5_M_5=HcD0NFZj!J_$$w9I5`Q;oY4J=*6 z4EaDgvh#CQzvH7;S=>}w5WaG;c~a0H_^_LxdFQB^^o%<*ze701V(AFDGof4rYHps) zWNOEk@ZdRiG9bf)FjM#pMuJG>?kkBlP=c-0Emr1Lx2vttFxW(oUcw_L$eXQqq{REU`TNs0HfK%k+Ui`POv|0 zb@ucPQqlm<)Op=}m30|N2IKEf2_OOjk;{LJTVE5HRt&Y4IV*wkwe>usQ46WiqHPZLv{Z`q+N40$ zPrIOdTuUARItgi2W>JVpnxnk{!dJMYmp~B+Lbw-pjmDfGFoLCT5>>4x9jYFEr)3nP{AQy_^8An%KDP1gPI5<+E;=Y^~fAyjg6#9)z7Y z@kB~b=dUR16H++6G;qKG;`53P4jz`0#nTIZ1Vq`EG|WCsuQJJ`6BV5}uD3}6N(X_v ziPuUefFl*rJx5Dv5Ei!e?9;%`TD(XixQJ5X9%PBSemGSaaARiSSJ$O8I-|4*MR1&f z{4MtwUC$0{Qr?yR-C>tk^PX5oO*04p8SEfquuECJTDHA4K>2pp@!)k(!L&wO%rOhk^CE?+oRl+Ugnt`%W0_?V+HF{KxV$Sf-wYef+>gG50N3ZV%H&<03jhV5&s~>v&Lx1!qk6(E9dItA14isHPh04r?0iel99!mFe=bSGi zs=$=Y5L~{|TdDLJxVX{rT+#7~D_78Ftjf7zY@@fU+Y%Raa^1r4pWR#i6$l)@!9GoxnK^Yt_=61b|JC+4rgrSUyxvq+rz$dD+^aCe?({SJR;@zlMo^VCfg1&Fk3E?nFXi@6o~@?c4G|bZ^iu?i2(d4=k#DR zkFsEB=IKk%KK_J>E9cm2JOb+)a@K=j)aJsIw zcfNTuDbzSA5VD<|DLf6(6I7_w!jKSnI=FKDjOSZt{@Ub{njcaZ>^Ifr@(Eom57&p2 zcMu)ZC8T%c5+yeb|1?P)6Yv7fx%O7o!PFE!It z6Nysx2ixOh@UHc#8Q%THdoeD%hl;40;jheD46 zWI{+abzxo~%QX*N-XVl6L+1nCw3nzn;CK5_Mje#qh!+M2Ou2_9R4ffU zc(d(es};*urEim#_Vy2sF8~c;&5~Rcko9(V1=x_5asYrdgaYmO`pFVjKe#rNlu>pX zT2jb3TH+-=NYQ7rBr=xLTpVuU&3@eTl{hCx2J=?s1R-GlJ=4|1NGyRTU;pegSFU&H z58+TG2N%1-q|ExSDL_bb&~A}PR!i|!B4PYYFBWSL-O_biLef@|P5LO(zZ90pRB~{4 zN07?&8Q*Vk_TZNKe0WkkvtQUJ0^TGoEJG|t*Ic;n(tcn8%s)A$4}q>;%XpHdcU}C*M=o8v@Z__XZc+=#7|_UArFY0FRzgrA5O>3Hk;vGJ zqScZrSC>PK%+$=tKu>p9XIGznv{$Z~l3aah>~M0P!Y>9bO1rBJO@f*lFt}k*9PkY>4VhxR!1{^~u^`TI8xWpu zKAIo|amL04r>yLO)zu1Wg6wIxW>jjo=denM3@duRwo zBha!WJITDZGe$&KLhOr%|Kc%*lw(wguO{+Awqaw|XA_JbCB9-xIHbi?U07j7bh+Ip+fK(yqC~ z(lB*oG;;x%zxmP&#Vd^i+?D0*By)*8yN$AA<+UuYAXxhDzn!&5S-?zr&O#Vg%igD{U2y!C;qu1G3O>|NEOkLl3jNhqs* z;J}D+c~1IxoF@Uk3=?+MYWiJ9j3!8{C(nIt}sib)_1&VaN; zJuS#w4s-Mz=9p?Xpp zO$7kkdF9*98C$)fs{D+b=Wbk%97(#uqdH15y}j&-TIb-U|P` zXo&8A9sobUnQmaq23T7H{wF2p{>}cYVM|0D7Pjeyp@!1D^kN;R?;=O9!!cC3ct1t ztm!!}j2qm5)i~)$(Z%RGqtldfA-PAildY{ebRE{0i(^_8t7G9!Bv6N!NwAJj#DX2Y zwZ3T82>)Cxe$qqtr2_|cR+{d7+ih=18RYxVqjv(tYo(Vl72&R=PxyIQwODkDs7jknEtf*t38v- zJL3mlTfmZ1L8`f0b@M_?nWz2Xx#~y-OV0^O!X?+08U;L)b`@(gs!9!l6<%FtC+fA9 zKCsaCEq7)<8{k!$s`zPxbJ8v8q6{LvIXu$w#h z%ST6RT7uLseCAkr^RdB3zM-k78Z`sglNvh?t@m; z^%FTT4ra>Mt(3ut`gw408P{cPwS8rO&3u{8fwaYURaRTR*f1z$fY1RNDpNi)ksa;V z#=5gEP>+Kz7r8u+d^56r5DS(q7RH=wsc94#iuuEwN}0IF7$2IK_rKUXzcM)tuS)L; zKk(KJ5mXo-LP$EtBFOv_PS8@@Yy6RCBa0^$0EYC2I+xPD92PDG%!NXs9j!pzS)a3z zO^pokvfeN9>~QJDq*3YStt=cItfKwEW*j|WnHcQj0@Ku)_=zow9qsZdbD4@dVk3`ipUR3P*j6g~L{IW~$Wt@6)}2551lv=dNRm&y)ckhT z2>9zp=-0@|VGzyEjSTVGeZ=sTY&oFW8k`C^pcYWR^_zqz&&BCnO@S(b2<#ZG8i+6~ z$Bd8p=pO&9)P_3^?2;6*!o}4er@?Q(r>m(OZVpSM$`3h*4qRb&Ztf;M|s&f&~(GO{Zm#^DD>jX@L^GKHa@%Gif!Nx{n zU{TK+V>n=HW724t6j5okw7QU;@sXE@fJY##+H7TF*OtS&UM)xG(&%4suL6mK+X@g_ z%-t!=^Bm-;1qTG2sUXuwdMs?dY>ver9aTQsn2B*;NO;WzQe`n#JFF`VXH9EHD4Cx5 z)(6ife*GF7JM5m)w}0ns^>rYa!y~9e-xC3r=9A$}EBilo{%qz;+HnrDwhS#5L1Jy? z{Ax8Ml&$k;+do@G`g6J*NzS9BeFV4}4Jt6Pb^6xV-uX@Y*(Vb-Nk_sDqxd`;Da+VN-~u2m0-j|!Esb1!DU;hnUt)B`rVJn$hc(wJEXkX|n7&S6 zJp@7@Cg?s!76x4CV_Z6dOTdK`%UA#|X3$@^2cQEV@Ht1T)HT?*T{sQ~8CFY*0Ui-?sEbmy_s@J10N%1?+WO{7y{%G45J|t7F0BLw}(>4*ug@g9v@+jnyUY0u9&@Q4S_y{7{U^}JJGyLc&1zt!^;n8!hK^13b_U?W1>px@k zpFX$^kWvF>0vNp4l+`ghHxHz7OJS)BOGty?$VagaTD~9hmN57TYJe}AcgvonzRqi% zeZlX@jOfqAKcycCCZ>tGvb|Gmyh146)}uTv)xCTtK@rptoCM{c%kyWUZKZf}lQs4< zsvO()^>N>hiw|#pvskOEw8L~Md4F$_VzYN|eG%EkC9<r1S1Clmo|HY zmeZ&(q6sM+10I=2igr`1b=7D$Gx?=XMOl)vGOJ5_BMNgMq=~~ zM!7Cd7eF;!YacbqSompyyS&19({~sDe%p~^(AZtXSxsHL&^k9~`0HPka1+&ay46m#z;22iO#F3U=!~y13clJ*{|c&#HD9C28Vh`U>Ur* z*+0VV$9$gHlIw0n-Vt*SCA(B1OjldR*&14CwKJd2ev**1lL@&D^rf`LLhLDq)e~Ktd&r=jNaN4k#c= z0!WCnNChSzl)dR`HL*rB*F&cU=?1;Ewqi50INI9nOYJKkFXwdfgA!ch1rs(_4P@sWjS!MF zlm%mlP0+aQP|dl_catH_RGLh{^NXFo8&aN~(R(k(&Tm_~%4Ynh$I5?2y2sD1t?llL z;zM8AD|QyFhA%~b4iAsfY9M~jGehTMI|ZT`ggH@x0@qgPI8osJ@?=g=T1L9 zsYVIPa(>0kSkPI)%(pHUW{ejA9boq&&&E=C3Pqan;6;O7oUkZRjC=)k5>2C|Ga;Ym zM#HQ#-j^}3G8Fh7ly`lN2H=sU!#8$Ey-!NNEJa9bMr6!&N4$-^PN-rl(mxE@eK&J1A$z!%jGKp`e8WjdVW-#;6FaH_GGIU5dg>0wzddb$C3Ll0pc3!1P8 zT=PV343Mf)t~ykY7J@)81IGnU;{$Y{^1|6P^&-{HAgZ-$c`hRvaY zbA#uG+rBkFFJB{mL$7k*fzZxE?^xej92Fl_Qq1(~XgJx2nTG_)u2NDTcI#Z5WvGPA;soSA>$J%mNZ!ejMR&zO!=Qpiol-k?8 z-6d*w67F%tLYBjI)Rm!tQ$7fi4ug4LJX~xr9gzBzEbLqCy*@E5iY+dE@psP}&l`VO z%U<6-7>Fk5+QRBwSRxc)_SpMNh=_7>a}+eb!lhgF(3h;`rGj?;Y~)>+-ThuKG7D!v zybaQN(pfIdZ~y*T{lV=3J%Zs|CDm4_2pFQ%oTMz<4g~6QRv9VBb6;JIiiVc~&5jNzCiKK(=C9yt&?AYeU6!oTm5$It>qmgpS@RK%52Ea9?E41SHA1wYQ|W4diuH!61EekO#qq$ zQ$0A?8xpeREw$&i371$EdKbki{bf4qg8Jv&jdi{samj^YD#|H5^@+ zioaG{xJ!&WtNu@(j;*F6j&)@qc4{Uk%bym@uci&?%*dI%fnCh#L!OoTS?f139JR6U z=H-sg&Yl5<8UO<9r%hpA z>ALO4jEp!J(`~n>r;m0Kv?(i4)# zaecHoKQ*A$0(aD|U9qovYrQ4sB*dL%!BVkg95NbQ9S}_50X`=?uiS!EE^uUX8D#SD zK)dlt#(@bK!Wwo+3nSLYqomL@EV?FdI0IbF5FT%Jq*4r4`|0+4gQY)X3~*|)Hum%1 zJ*(a)bu!nknF~fE-)JW$x#TE-cED6tKg}PCkCXbU+PiY%zK-Xv4oPb4f)Uo=T`ATZ z=Ex=60lSSc6|E7)wf5WS$nT#uAKdo)EaYhdvD}biaX6?F75z2*48fg8;Y>?6GIB_tw&i7U4{*WynQD5!yo?ME)#hz9)f=vhhH7MShZp?wJg6 zI}$f^DeUP(I$}<%>ip6*Itzj=P2HSr~^cJ7WqOxQ3&o}2;UX7D(ny>sr2{s8h+M9 zXn+@sg%U zyo1+MoN@_6H;js3mu|1xuquC93_y_U)fjUSIx0f%{HDV&|87=#udm`5BuuibK#>Y2 zN&t=DKO2508Ytww+6r3)YW({q1v?!UKvt~qo2$Y6nk zsT_2(uXob}QZhsOwTv=9X(<#hv9c7S#Mkyf0L!T=Y0NZjy2?;WrP+-;#p%Y;ra6<4 zkEH%VI(#BA+DVh3%W{jQ9sB?&t*kd#i}0Yza04s6MOMK-y4oLHyah=k9xGLmp!K5W z^iz(K8H|=uT+Y$>CR2Ywce2%*EKU7_K9DF&X1e_H`XZX5Ol?11F81fOM8XKxJju0c zP7QZ<+@KM{VwGnL zP9VHL2xBl3Cgdfhotf;{m|)?)K5OEiyE<{XoL8$K})k>pd zd&5H#@{|kiDxM0S>o0!%%}*cyUHjRXm~C86zh~7V=q1lbF(&s$ErEkzCdtf$p{<%R zw@Q^}=hf#P|C2wtaOLvFr=Jmzdh9eNe(-4U(RF zFBQ$2$w8ur{oVGWYpg`?4W#p;MwI_gER+jNeIbD=NlXywgOUBZgtj8@y>ad8vllNt z^{0RIU;pzT{9k`??!Q{g`Xk*J+$8U%c3WO{yiI6}T)>P_zug4bv?MEJ4-|GZo$%3; zsw^m7?;oy|OYXQk1n?_QJ^j=(H#62~lm_8*bSCmJMmX_Z&#%;FlF+l2V!ozs#)~T^ zUWY5{EkySTISkfPrgHfPw7N9OT<$KPnewZSq$7k=AFdyNE8feANKXOt*S^VPn2= z{qmFl{ZF5EXrF%OrbKqEm>N?}IWr=g*52z}%D!=`{UiII1Lb9CD5Ei|Kp;)#w^eDUhrb>6sPsc(dyb)j`RXHf=f;I)|<*%~smL=P()Ad{xoC{WFd*APX+8atw=JtH8JR z+;ubG|EF(%_70oZet8dQvtF!x3S(t=gBt6;fH`;-6Y5C8LjImha4 z|7av!#z=)j_gs0RtY9fa_tKA~T&embe*$u{Et0^Is+`BFfYhPnVQ2Pqi{sVd&MOx$ zJoV>KUhX5%Sswt%pi4D*np7d?M(Ez9|24A$#R61gTra$tCX(|!M}DL4@`peC#4|l3 zvQJ%!)z(-w+yMD_U+48JS30is80gQSYlxNl&e7VMtbL8mfAi5=f6u_k*MCpOM#NgC zWpnA#z591h4pRIPw2^T&Upap^`^nLfUPlL8yE3(N^8Cr#RHK-vMfzfSDK<~5m`B56 z*gAdnqp#ev#>aKkdgdta>;o9T*{&Qrg21Q4?J(dYPQDQq#KdsV)l1J_zH&oa#sPA% zb7W~I!fI4*g=``v?e30b9Xj~h<_hJQjIyJ%6^kF&m_e|kmnR*_=mEqyn>kS$)b!$K z*c52Awce{wKK9t3fB3oQu3qb{Xs0Rk;Wg%P7mvs{D56w!8GB%_~eH^@{uQ>{P1Jv{`}nIZ5s@3n10Xhfrt~NiNM?9Ug^MLQf?Ki)2^ouViGZm zf5LFl`b%d7FH#6vGW@1}Z6=PY+;bN{a^cDVK76CrbrnUfGK(F9RYb8XM0Y2AX5RfU z1PKCiipWScKb9V~68Y+dr!QU|n&da}b2gVIMvRsL9MS191c+WmVp-KFeDOO=TYhS) zUfTTiu}WWM>>KX+%Px?6zPfXG`ohaE+&LB}GPdZ#&8A86c4HiX)L)_GH+yjB#oL>y z5X|NIChM^|w7>%P)~ll{p_6wchV_W1E{Gn`?%kX z)Qu0_ggM^qMP!C9L=zBeuxY-9s3@D8V5klC^yr#Ta|;5U9h|%SU=gkyFPaj(m9Dlr zpIE4eY{JC{pn&tO{dG8+@zt}&pKe~faN$WwL2qBZ;d83AVSjTnMczS%=aVRDrMu)n zxKK0&jhHTD78jqna{0nzk3VtY!sAcK-=1zejA+FQge z-5<2C?3$&~!&eJ&n=;kyTh3n;E9CT+;a$D*m8d zP8bM5wUxpnM8nLT&QI3{u0QkEg{Ln(=Q|8e5b9(aFCdS|oRyhMv%d03B4wNlh+{TF zoMW|aH1#W&I<7qZ;m03;lAAd9XXifBc2w%4`SA3tGaXGT%`z0cBR!6EvSgwB42WlO zzI|?_1bs9g{8r`p6c?31q3X#=d&rt(B{H%o0z5=pJxk(gV%X;#1;Nm1A+~jJ za2%bw!=pRa6I*}doX(9kR~v(Uy4w;T-cC&h61<bAU<lR8{EcpzrhkMERpUcL{Z7?26~Ko-^u*&&8|C0d!m3klJl^kg06NaR$LkWVJh$;+F11(7fk*$ot}jkV)CCe`Q1@MNgIFp$mDZ^Y!Miq_JV5?C+h|Jrkq z<}qF7qb+5i5onluA%JW8&`v|rJ40w1j6BAMuTH)mN8?uVBds|2=RUg1jMWVk{%*Bz zf1BWCsRM;}P2GjU|Am;$HYq!|s1o7NQJ z=4MtiChsQ+byHNobm82Eb646%eNxzg8-0Fe$DS$%m!EN4A{MU#O2&}Uj7ZH7(9_^= zd=dW3?GO|n?CRxuXE6h5N_%penaI;-bhK>2;{z9~_r+=lhFyHH1EQ4O6Z;QQsL zKk~!{FiBSsvTDP+x_=5?8HAy{;|W5a1|tgBWYRa*FzgZ{qzEtvDqaz?Fekr%e7s?U z*Y@U|>C#e8G`(&eXLCN?+G7ScL!W~%5SNC-i#}4-T=EB>Yd@>XMrOZMoDKct8~96i zQ;VPe>VG>Md-!)J77xR8nS1Q>T2|_v{eSrP-#Z(9@TqwYywx1@2R{X|@pF4${@33- zoBp@&??Z!fvXkgfdxy;jr`n7td>R7mREm;WlV$gGIFGz@F?$@0LS>>E!zX6oA@kxM zCSf%glgyS3hp&=J{$hG7k~xMwO7@=Q-^?M7Cyo?BjnaV+x7R-{m_^JZ-7*c<&dV2` ze&)GnpLy!3$Dg`%qc*Mxa_~0Rd?7xWxadfAusRm3VZuuwg zRXGB5&@@hxd-^+KZX<4c{Ck?Yfq2Iz^y~eoeg;5Bb(W@XeW3IDb+E!k*{L=u{xZBq z@4)4-I4+Jsj+`{v&+}~S3XxD+jO`B#*b9Z~&plT)?;YQ{b8GkLU~gy9F_}Hc1e)iU z-5D`*k2T(0aoNk|bW*rsEw&aUP(u&Cz{q!2HkQB2$UpriBR>z7p1oTd`FnS^Y`WRp z!cu)?S=%Q4lX+*?|+q%$A0~TQ=Gemv0|oD#)|L%@~)~0BRr!Z z(pJ?y7KdDL2L7)57jMf_*q;$*jKZec+<}0rhh?V!rRq;Jx@mHm3aRhLIdM-ORIplO z@Wo=IrSR56AoWnMj)hm|KPBgg{A7b}b>JR9$ z($C+TMyBRk5MCpb9r1sHKRvzY2F?w&;aRA=0A?kI$Tt;pZ>X*6_SdGd#RtVpxt2Ow z97)Evwf+Q8Q1=Lxvo4t3#VeUg6q1LLVkrtvjbw3w6J!4Nv#jhcv$Cta$;$E*OtQ?SGdF9|Mxyrx(!Ex;5^Z9r) zQ*~d)p1D+9f8pBo>o@v_ zr{}aFme=yQg3FWCGeWD8*u)saS3zG(&i{0f%r|-F4Jr?-&skkHR8>Rf|b?1<2@7# z#IuE2{GQ2u-~Uf% z^>5)cxKZcN27gw}4Y!uIe)6BrCg0Tv3BeZ3xw7`%lv$^Zw)hCaU+$e^6e zznD*(RBSN14a**;FFDdSDp~LyycN|m9@c_`lxpeAEv?OpCaGu)*->8Q1G0=E)2QaJ zBPlvj*MyeVYrdPbk|m>Ere*BCQFGGb(cgA*(a*eUkm&rQ)Xz0=5k09@43#4}D1hPc zzSTj;_U!mqItI}Q1We3o<6tg;CmubL&0B|e4nw&J!@xDJJUD)beZSG1SdM)^^I7(N zK`VIq#n|`Vql2Rt_P6kiN4_e>YPDYZ1^a#{^KuJw;j8dd3oAQ6W#9K+3}P9&AR4Yd z$l@>WMJiY8$=b_Owi4ikuFXm9xPnb5PwGDxew}_qIoOe=s{IxtLwx0EQT?2S9p>Zo z#KTPjAzS3v2sdHbPVtgIRwCS7I+Eu&ia~~YQLrQe@{b@dO-<&WKHJ%zEZit1e$n%mG*p+kqOGTByv54EoiVne*WoFPwQL=frpjTTpC zvj7DsYrt5`z>%;AL#?f((dw|4CqhQoJ1$?5ZPAQ~M3Ashz$7hVnagNYY29M^>!t~% zD|$QN2|T|L9R#dor|ftLLReePUa0*O#Qu;&Xn<;&tgKlG<5X8Ckv_EPRe7uH2e%Ix zH=-SjQtZluqZs#ZXD3%m<9?vX`4nKkG%ho* z#kfQ7zF4e`(nMm>jdjL-_{)2U{3umY;P6tVzPg(`8?HuDy~&(l`9KEi45;WhgM55L zwaw^|^N3qk(J6qPiO(6lnXu@~b;uO>@{V&21k;y%#NUY$mz@?S&Rj%&RtfbBbnIpq z#!$^|+Pl_~uK2U~X%TPM0-~Os;cN`6=$oZ$gfI~mpmD?uTIFuP%UeG;*4A`K32xZo zE;i&ID;NHOtHf4|v}bZtVRHn_RzuR>GQ|%zs>bqt7Km= zQK7?##7Gzz!9tnrgd}_IZ*&^p-+lez+1a~G10OBUD!*xc;{4gviPazVJHI;n&3pgw z{qO$6`yV_T{CKfgSy*y|hyKbs$)OEU%J9GctxiN>VahFQ~ zpoBGoG{7rRYFL;dqME^-`IT9DVhVY&aMsP|Y6=)6$^rMER$~yu;D|z+0RueDV)Ctt zigi1>Nke)KbT%_IY_3EWHtgLi;$)4(^s*rZu_La&7Dc?HGb9C48_%r&{!oR^2%hN;vxx zGi@8wi@Nm`_hjQM6JL+H4?oV_?=26O=3aT5x$n}Tn0(<^XAgh=k3aa{kC?muw#LHh z+FNY;m3@I=zclwxe^{FPEzf3gIp&rWEw21kY3_G^bN;OH!^AZ~ciax%vWI;~J-2KW zd#l)-%Zv`v!kndHi}SdU6w(_OXX(wh+?Ts%OG<{2c7isekQ){;vVfb2$SRXa(V$;A z+lu<7Y3=nHvdCLV>b47Hz5%2nwtiN(%b=I`699zpENHuKFTk+V1zgqiLU0IXIGp^* zdV*j*p0EMleJ!&PE710HMdnDpv<-_Zd&SnMc8de0htj?|P5MBNq6dHrHBJ$wXuhfx zD5+79&CGhhMmTeoH^EP7K2g!AoTc#>LfN8Oxw88$fh;nRVP|viFzPd>xS82~*Li1W zzt|f;I61Z5E-gIE8mn;Ny`5tFVC&9{Cm7GppzDx*N+Kn2MoT)>eN)0)JBLT>jndQy zZ^YE=^*3^}mt*RC<6_Kxrhe~7O#OhVKV_0>ee;tswNZorpQ%$nEXCAH_urcO2aimx z24)^j^VS+uPd{Ml#nrIPvwVGTuh?weTC^CqHLG)F!ryR*djaDtHt)(Bv?(-mC{FtpbCYwl@crllQR~hf z(~3nrm2E`9*#e{aXleP3`B^kbVSYZXVaYEKQXQy_mvIl)hlAtZ%mO3~%$`RweB0Zd-793%~Uah-{Y&&_a>8G5XkmHDq=x zSE>?|n_q4}8-9O%@<&Yk_T=CD=U1xkZSx@P}3uY7=+Rfn>A#_siV)>aRYY zHY2<7u}f1nSjWNhSDX$qp14+O(iD5CF_{hks5(3d$_1&$Tyq00^l+ z9UvZqiKUKZMr4yc&Bi@j9F`Z$rJo{jhUbdqieq8y9$}5EG?S^WAyFzLbqaZG>U%4j zyGlp7RD$f19pjS=NmW`pv)Yp*);b`sMZ5-yl(Tc7QxyhK!IdTB^2p#xxQ&Y*X>jl~ zN0qFVD$2E@9^aIy;A~H2Zs{z=;6lUj8rG!Q%bkSSf%HyG5W3M zKgQ@&G5S|WKgH-L>W+!S&mPQ>yOB%0`Y(+5_u6Xn3&6128$I~r@##OgjmiF*(&&3F z+`(LM{lwK5WAq1~6x2@snbBVinL+^8=tNcc-ml&|Ik|l*$Y2F;wCEp9r0fvoCY%KC0@tUFt7;2S-Qn5!+n&_^sVyZXq^gKqVEg|6#pIuE{_sCceoFecaroIo zf(_2w+-s%D$y&9R4DQ$71S0gsxBt_L@A8*1`OgjzPBvq59&Y84$v@#>mnMHHT+=cg z*TH9|x7ickr^H zE5=hpAJ!-{EFOwghIa!7%bHy|DhUp{u8%fyu6g=mOlgpuxADrRp+f^;H-Xgh4dgBm zR=agI+zV`6U6(}(c0^L^iS*IQvF_;Ukul42^cvc}BB`Vfp_yUG2ppd$YYdAzg@m4Z z7?x?~Fb1;A3EvSOV~ipEYGq&`H$o5c&2YX|(*yk!MMsB=zRwL+1#E3F8p6)FZy)cV zs9Vv2C#VwMg*rP$XM-%33c@HwDhJ zG&=~ac!?5F;2fp-oyv;`iI9z#jz-5q+w*z{_F8!KuqYGDp+cAl)JXSR^F}&3f3(G@ z8|l`msRWmz`Mh3b>P}q`R9VH}G(5jC<8hBElRG{)`zRi zHT+)PkF9NoSS?nsC0GqSp;csX68~#WKAW$w7RnmrVGK3w>4#GSZ7O$lNDc)(-a{5fmOHJ3WfVa z+`@dM=zDZj98YFK_2AaZqFbX#E9T#Fd~o%&zeIix6wkwE0&fRuH;jV0$T??dzT~KZ z{ZwDj3`yaJS5JR&?cCbA)wcT-cC7J<%`JG?Bn=RfJPSrNn{V*(0DI?46~d(a zJ)0j}9Cm#7{+-iP+FvJ#WCzGPEbd$wz1>*KnoM;eWUsENR$Bf2-TEV|=ceBom#sH% zBeG=i^jK=t|FF99cpW0n>K&K*N~_=3f}d%XR{!Mw!RBtPjtSlR0;_*X0{b6UuTRYK zyldN=vbb;k>do6b%hQq1gm)UWot$BHRNrBX&r7S@3!7en5{Vy%l83;K^Hq8{+f`A} zHjNF08|-POgH{gIg#Ta&lWtwHS=~FJf!BWY@`3%cdmD=_V|hG}yX~rRPBeoM=ER3| z@V!bhw#2Sb7h!W6?NOUxa{yooTB(uL;~5cJKex{4%huOe8*I$Wxanlm1i6be1_)9Y zjJHpG*FZJN_3Paz0}^*V`OGO|=JFeyMM*0{Vwm%wSOj%G3mEWOt8AbKEv?+F;e;H=(Dg0NJ1=3XvcZnVu|13XMs=Ag_}Jhdi!# z9B2*YCwQAEoK6I^%XlJ+FqKC4weRk|0W4e<%{z} zxpTFd)+o?LhF~9C*zwV&{jIT~MM)}5X5|36K6d^Yuw3H2r#}xjK~ci-q7kI^t#I@) zqf?5V%5&vICOyl#&2l?1I0oUcuM07*K9}7yenMbd!rN7)A|orolB=n`?3D2j31tu= z!K`)P{8Wnh$hDOK0^bA)4jRE-@dSL^oa}HhJB1@`5js-!!JXM#zxl1Xzqc|6ByZf8 z$=Xc3`Pw&r{L7g8pLgpBJ;zM?1yguOzhLg~2&GsNE_L-A*vLw>+RwSqh0@$>hVN&a zxBPnIAD3h*n5FVyPT}oetaV+x(aqe`O9yxF9&F7|m-90b7BCwi>lw+7PaRABQ1)B8 zTNFi?opgq-wn4qkwf5z2!(yc2(TP#DISpyDaLK;6v7SfN5K?D*d&dV1C9j-=){sdMCJq;u!C&TY04@LVBM<~7-S zuBDPfvX4w`Hd5@QCqw&gu$LI5S$jB~6DwcN&Olc-GZ|XFKSx~>ocH#(7eMS2Roil8 zHn1b*7Y2vJ>QklWmOH98okO19TupTReJNWO111Va;ng$snl6O1?L(qN`Kz8lgrDTE zEkn=CvSX`Z30C}VdA*yP|I^-ow>%3sZ`=#=H9o%ezt~$)I%U%@u1<{|=6Zj}#KJ(& zK(6;Uq%f+p+I!ow%YVxCUj1L}y@0sHhpLa*`_B6ty*E3JtW8eL?;M}(t`pv)&W*%a zqcv}ws#4GORzTsEi-^VY9D7DGR}@VjAa{_w$I=gdTaGbe18nS9$p1l{aU6Z!OcLa= zC|Y;f+d*m3c$-utr-Wnhv~qIkB4(x%+Y+vkT!czSPD?_V%;Krymd#$97J*5hRf)H@ z&uyREYI}_`T-~tVqrG&3nnrO(j+6eATS-%8R4?;7Wf;=IBK2&@w>)o?B;tYaY7=LQ z3^NF3Kt?MAmGF?@?S}dK(9!y}xNZ3w`8Loa&Q*W9Tnx4T$fg6RY2?Ydre1cSAH#g%ADG{K_yWnPn&5 zT{UMYZ($4n`e}JW$U93?4;TVSpgySm8&x0^%=sS#JHr5uD_BX6Wi(7<8P}U<2%Mf_ zjxG8=Uy`-15i1a4A*=j`o2K)a85V;$IF^jMv6oJR%L0EBi!mVrjOi+Q>2O%YYuZ@g zJToz7|5kIsWqOvCvVG~RwO$=08k9ke;m}(jSZf;_vbTa*E9kqJugszb%sot{pT~^v zj=cs)Hy7JYGcyNZ))`#M0`)XMQ~_!@y@SXGsU{3c*T!`3cs~QF=KKdM7&P`@tJu_V zgpsVybyXeQ{r%J1w@*MU$G7huuUpCDN8nb5+>Wi#kdB94j@gG$x=6-Ot+75=v9o8` zBUXK;5T6PGDV}`#?-}}|UIqM*NP~czy#{* z_n?CFxzV?5VlEABuy&YsgwhM)u%52H;oO$C+uQH&R8=AKc;EEb_O&#}wfD6rfi9@G zW@@DwF*JqU7+U>FNN>g{N?MS~Jd`DS+wJRb!eG{yeH+#eiPP7_TFJ^$2^XCy`jhKh zM}TMVvahrMXciiHv%4eBN^OIS<}U9ZMk&kNDOJ5~>xKg^UERDiifmH!{EO>kvf) z>c9>dxIRW1g(_G%bFg!;m&UE8M6iQm_%(`6sC2PqnjRmUb=(&~fT&B-6I@@r$iwX!B%^S4&sHz3@- z7b{yce_C3Zd=)>iTv~Z*0c;*C9~`jq;4jZND~(A;op^V{6VZ26@-z8gzQ0YY(USU2 z_0horI=-=p$Q@zIXn|TZ3DYsf^gnHkbM+rK7FlD2j9X@B%g2Qz2pMc6q{Db`nOLSG zq*|66E%L&tVYhSsyX39$of>H!_|@L<@lGDP%b&s8@WpC={zusr?$6%g z;SRMsM}LmnplC?!pZLYDu7i&ug?5x~J$MOnue(4`L_(B|D&tYfpvBJt>;90xaCmm!k!lt3kcqJKFcx+>_9=`D`p1y76_7Tu;|FwXe$PxJ4d8^0gG` zI3XINs5%l^2sbRgT3nFOsH^q6)M&C>x~@LDZA(2}!^piGt%ol_df7R;;N)7xUOQW6x!~4>J=vPUeyG}C z8RM+ftyFdRRSp#OOyovPmzI*Ol|w@kG8PCq*#C=z&FSIm&pz|?v&6HnUw-x~(VVI- z(X75~bR=nxXcm^hfn}OmyZiq#b>Cl>-qo4tTf8NE)|&eRf74y}u9@+SZ5RUrBoGR% zZgmb7x^m9pR63_}&Y`1nmPke-n;-;EI1+4ZVeM;!cYk+y z_OqXj&(@3WM3Jtq2a-1M-r>aPt(^0_(juMg!KcIa{BC*9hg$H1x1Hr1-M#(SbIxDQ zNc8B)xbOdIwjBApUvO(Ta?Zcpvp52L?O?}#b(l6Uhg_|>&$ahoR3Pxo3AAp4)`g~( zhW9hV5AV4dS>RoLz1?*r*u$H+bHE~0ivV0wC9cz1Cqvpkx_P88I!=)(JwqbhlF(x1 zFPxjCFiA@ZE@tvS6TKQMh>HvMU2BMplm`t&mYVuIuj|iS1~?B7(x0=M4-5>7lNUz6k+Ccqmcwg}&|{{Zhv?WnBQ;2;@RN0}Lz1?_#M$Om)^SIHx{gdVVmS z3Al;%W~QhKWWdDN=O)JSt7PlgyOOS2$bJu-SZD)AgO0T$Nkm)VWugd!_Fkb@EHGBu6cf=cq|9?A|VqnktuyI7Gs+U+rfI27hEWM?Lob zb0MXGy5x|>+Bdm9ioQAaTo>8;)A*9p8|b%*vGLH8O#qGr>mO<$Wvb z^&K=)N|~D8uJ)mY)$N1znGquylv$$0QG#ZRa47!-_mT-|fu>PAiZkBPS#(v}ItSYt z(7hWwdeDX^iqXHe$U34Cgg1J%9|7`UWX?qs!dh{yKq9|VubeW}J*qJw>=4nL7{T;~ z%HjDqX^7NlR#mq)oPYAnrHl1VO%3%|&oy4V*45WHQiSb-rD5U`M*vZAg35(qNWm9f z4=gU-&Iar*Q0c&X#}k7yf9vGl>>5npbKogfyZ&o`c<1#OpMB=<($G4_?Ha%3OJCHn zzxD4f_cvIQuBeml|A~2r34cete01pF;=~Q7Rwri0;>3TsMS$N!#@%56RqT~OJEp{I zH9V8_kFzU_FqfdJrdnI)NjH)eBg5_=>-Q8JgF|U0TYY59BGi_DR10~&uM`HwK9K?9}&pUegVgNSL$ro(1djF;6$%BVN;vI)2tCj zTrN*MF1F8Pr8bEKS>26WXUCHbi~XH8g=7iBwBZ4<#)-$}`AkB>$`)S5R#MZ&+N3Pl zuDh$}E?hy8Y;9?2Cco6$Ghmr~BAJtpATk;EQ2D1($UTDW7aHg{rwuk6Al4N>{{EfW z@%g`VbYDa4_@bkG7Tb0+NB+alzx4d`fA7fOznN*4XME{PQmLnM@F}rKo6$4gV72wG5$4e)h|K8J8z-K+u2Aymvy#^(Jx^RmlB{if~R z8~U`G5+)oEkMF>GVS=TIkrw`&;1Di2B)qj-hx^Ci@1yHShe!D`ZQvQqLhVv>y2653 zim(h@bEg5%vX$&W=PDhP17AW!QL?X&5Hb;}AyPPAnNT1FtOY96c@BF!^AKtAeTD_+5KPt6dbMKKKL=KcRUX?IEHz zIq-=X9;cK`JIPw4mrK2=&HdlN%^`MG_`IyG*X})}=;j+8! zj-PM1))e(-=imXGV&H93Y^rFGDZucOaH`pIgk<*w;_n^!t(npCz`trD^WK3^O_m4l zjz9NYdEhr7k&{n3)Gu;hx61=}Jv>3}=|vOy#xqXD4Crxs_!MpsMsJAc=GrY>q5nd)Wdy1PiQ zD6`XHix2#24ZrhwfB5I+f&K|NY5yJDtiS9U|Hm7+l#{c6o|C$sobirx{bMfe_+5Yd zBN->U_m&^{ZaaY;fAuwqbH>ACH@lJbXHg=|=&DQzHRRwR)YD4hf_(T#?GCkB4JU{dId+74A8CF4b8j zOgbd~ka0kbGolQ-P9=oC^7(YRyf90%?ocg|Sg*&R0$AT9E@Q~m+gYTf?{*A~N%^DB zGt8Ctc6I4YV!WUwR!@9e9tsv>y_{{PrCcxESPxi=KCbo{$}(&4?=a1+kSg@5)WM+D z^2~KvO*1S|ht#(XgLz`SZt``WAcE(?e3H-ly4@mL-^k8Bn(eN0Gy3Q1tnR*UGYBeM zNpl6%oKR)^Y#|t7Yh;PF84}`9*|}7J=+asa){~6Ky&HIUZ*+X<#T@zC@W_nDRsXY4 zpto}5UsmK)LmwS{|CW=UdDgMMiKK8#Wj*w#h%Rt=f*k#C%UXK;-9J_>#w?GV&I{qS z`M*`m|6-piS5fQZ+mg1wcPm|GS(r?^Zh0Rbje`K~A$mrtY91+InGqV@LyQ4;pRkvP zSoDANj5lEx`x?rdn+F^^p{~hQj$}T5u_CQnTnQ7Hu2nbnje&{TUd4q_zI5W-0%W-( zOL!$eB3a@-XXe~eqYC&)Qv>ZSZC(BJjy%E8-)shzfC5Cy6W3U`3U{88H1IItc4D-c z_!d8?4$F5dOPHoRIyTT}C{~@CxxRo$IK8%7bob#YNTyZV15_5>^0CRqs(i_?g7FLV zbj%IS5_`>zmT)4Rl3Fh;=jv>(`Mdk0WB1N{b!c!>i@N_=vk&E&f63s&>|A-~H`7;3 zEdPo=x*ne#ymV);;_bFJ#A{3#%z1X;G(uMvaVGZ3zW0hlbfR@GO zy?ysBtqay9Vv8%BFs&4ap4GQGbRtC*xul0UP5ueqEiX`y?e0f?cxUJMhIXQ#Sq7V0 z+XU}SF1@UwsWm;^Aq)>z`j80c@Ya@2d}pCT)Nn>OOM30Q9im&$i%;bhK>pnnl& zn3*hQM$)-9++^)wF?!RS5j6la_+xT^AT=sQI9RG#nIJ9|;Kfs+eC4vuNBv~*SIeFK z1CzflJ{Wjg_#X$K_8+!h>-d(t{Qi+*a&%dvV-Uyw1YKP#Q@?OD@48T-rWbj#(;BWbbTe0O&+>$2%9Anu9s;fyG$L+Bw~ zD-W4-0v#*}FJfF1|-T9^*Z%b&~taRIIR zkTuLs5YNo6u$gR5(i#mSk549wNEBuVjM~|X6MZfV_Oz*J+XODlKxE4#(1Pucrbj0k zo}2&B_rg7H#vw(w~cv|kp zg2HEkDrQiVSwhYir6Z9c6Md|y^)cY-ZoGJwDm0PJ!%A8p8{cD>@!C90SnPDrCZ@8c za6I2BLy4*$P;0TMjYIN1PxatA#yZ4U3h#8bXPjii^h?KK0nuD`5Sst$$h_+R7bv8B;aj-J?uRnczzvOU0+hABu z9Pv{CHDFSn_@e$di^c$F6SBown{uC667IHhNHqppI5;QFx+%I~?q#G+9s@gs@e`0f?qCdrA758Fp`pitp z$xr)WbFka-;bQ2|%({$JWh-7Z%SxQ0w|Vcxk&g8g+{|buWO&;}F3pBlqC!@Znp1SS z7XJ$=YJ5 z27~WzTJL`E#52&CJAS>htrt~!=-+NgyejMEiSNl#Cz7Z`I-Ot6Q#UJIw=T*bKo4MnEy@NIv~O+AXm%rN8&$wT^B%-DS~Ixx^H*bWczvy<=c2CDh|4}bWhqbyp5V_`B|N9dii~l8JPY2yE~e(y#PiEyb-kM=;%Yg8R{>cOV2?X2K(R`)7qv7<)EA>|{Ty4|Z6mp|aWIw7hdj#Rf z7CglOH-IDAsd=;x-(aa_4{Lz;ANm<$*sJ z6(^{y!Ff2}i|-vOE6i38!nV|S9wS`^V6eiM2>>v&1*$4Ms*#uN1+q)U=7t-``$lHE zcZ;nVE!6#;P;*h%Y7#Tgu-N%2S|4OP9uZ#3+8eK&d-B57mJZG2Zuyv* zxe=B&IBn#;RC*X{yVBm-c;?9lN?8pZ-cEYqIq{sDjUf@bl3gha)55xM`o5VH&*EQ} z0I`O;)p>j!c82(we_GH&Hv4`@+pO5xZuE$8|kayoZ9<$d>H6aWW zsbH-WV)LwPAkU%AniF3`6vW5dT=Utb=d1fiH%+(hp{KxXws#NrL61&aN$*~$t5tOo zPvh6c%+&L$Az7KK&uY@nYxNh;UukOZ;byu&58RZ{j}7*8k5sL3!8lghbT*I-y>RYY zOVj20Hlt7f`^eLjNQ2S`fUIV(^Z2+$!LuhA7|1ZShI|djRDGs6BI;lTRg{q69|6>h zsex#uSfeRR@!+)ybJc|+$tIw=SgtsK^GM7g9^U@qxxYyBpP_U2Iw;GlAaY0DV^vmL zYe!paD+O2ggsdkNj$Q?HB?Q@0pCu4(rVUH~PJL7A=auc-u=3Ren~ikC!M`WpJ4mDn zeLnz6dE(EE_O<48-}~BgU;OL4j*_w+*%V(I{E9SSdROyo;6MKC3(vo0ZfM|ZO_wg7 zyXfzJ+T3`pZCE`waQ7eIdil-27SG=sW9QS8?-XzMeD7cCKKPS=|MA=3|H0d@%7WC5 z%4!@*MNF?gaT8&(CR`99z`H};R@P^463#mO0TfN!h(DBU?eDIw=xreeRjR8S#diM= zlKFPA*}DqC+x@ehjWob5F*NPf+kE!)+0&j6`jJ#_imKb ze(&4EsMV6Izce>J6q5P--+K8gf14A3|Bk0V^MXHpHKfX;^2GoAOD}ybC;pB0`ua;( z{N2yUX7^5+A{n^*i?6==mJ=V(iQ6du!*|~K!P`!J@8AFWop*lp!#9KxJP)75L?-;& zj$radP1h7c$_w|un5wJn(pvF^6j0SsUBzkgz;JMrBkQYXPRA=XU%@CW_J4W+ncv&T zt}$k1l0*EL*;8juli|45(U;BfJh4Gv+5kN;QF#nYca3~yXc#laKp8k* zS}lhJqXt4ySHr-mmw-wtW4*oijoLyH5}dgpZQZ@>*KlMU#r7e*60l`@0VN2S0r0b)%ag%@Fwp$3PBv0Oa&rTCo#EBwYm|521(c zp|K5ji~eUhOE#fz$x9=EN2iDPK#ic8Y$GE-7vi%BlgA^)c7$*gxwnfnLGt&$ z5joq0N;%_8+8z;zC?@{IX-^`>%~Cpx)Gk*G0^pg&=9JNZ7pJX))2Zg^(RpNCDakY1 z3YUqXF;Qt<-M}(gN+<%~j-`}~d;z_kEv?~u9b+k1QRdz~$I!uQNM@PWOilaE|`pQQP_0Q8>sj*H2j zfNeu>QroFayh`C=@Mk*to$al@_|ul=wzk&VztHq}_i2x4U~>AyA58pUq;CiT)Z$T@ zhE;CeeFZu2j-vJHYt1wZbP7dviA`3SGeJ{PG3sor5q-l5)%muOPWg(yg%|qw;TM{+ zWyWX?Tgx+Jhy`sn$F^w@Q{w3C>LOn!$=5T;h|$7QHj!AcD@B-@Gx_0ep(A8AcbaMjfAeDcpK6T zgYrW2&^zYGE%5$K0X)r$6m4l%)n z?z428O4-nmc8=xEp1z6p7#W1C>{MbO(oIq+xGTwOl^%$c{sRY%$_R`Vl4@?$p(u1V zi^K14F3!-dN!{7e+>A6*YW8-v5lzdCG1FXH<-bZxHj%Jon}s7#+(&rbyuUO({ZMV! zWwYSMF&VXMRaASXram-%;sBDPKEhq(UHS5)E+Ml@Y3ZOkQr_bp1FKjbih!ykox6W= zdfwPoeO8W2K34f=ya{re(oo(7m0rpY1MhFm5arv z)0Pl}wRxPps2v;-5yJbCaXZdTV?|!fy4Q9oH_}Ds^pK^&F46eI?kSK{_S!0 zzIU?i#4*`I%EV(z3xnKmvGzsBnJ2hN>MQ;hQcWpK#Yp7AVphRxsa&R~fm5P~YHL=rOVdMPAT=<+UGcn-^TW%?x|xMyeoHQ$ z=g0wvDk<5r4|r^_qeCNbbX;u5m@HPFQ{bkZDH-r{(<-pBZu^1+B-N+y3)j}f>?RO) zs2k;!_Gr}Vbyj<3vCb&~hME{cdBT=KS8)rY%W&vQ(9g+VZP-4bZCWe;X7!t*<;~Id z_O>_v{Xc!s^Oh@&ppp+OgC2gsLuOO6F%i{K4d(T)XC3o6f7qm))IWY*}l^9fs;e=L@72=np~8!|DZz z?i?P`-#jLsad5Cam$OtHDUK^hu*_K=+0$6xVEJ8(Q*CST$L4l9abLOo2918ul#!t= zZheFZlO6}jN>>xjI$Q?F8PW$E%2cvZgy0yGU|~J;GYcPDJ+bWgctxkhy-{?B~Q}lCH0> z_eOcFZ+L)E)P)bz$dXK1^Ep~f^#_+Eb+s7aIO478ZOI|DCCuZjZkWoh+xX?C>*!h{ z0`4B%yn%6WbANmL`gN{0Txf$02V3)2v_*n%c4pWWZFWJsi3W#(+tGup2xvn2ol zK)47XP_K)C!k6w_zi;itO_$HDbEM%d zYO3eP&PE3=T__1mTE6is2!jSdvz)2}i5igG+S_ZX0e-qDuSILu!=ecz z6aa!B%9sd5?BBR~{rLI~vp2`b>q~sF|F~&MH|?&l-$fm;XJ^O#UGv=?gy3A&BpBLY zno7_^_v8NAXN+Di>*AW+^$P2{Gh$(mG&7AHmZ-}^hAf~&5a`f{6{8PK}%?cv#nFbEkLev%EEaKl9+=Y>hX0k{m722}!ENw%xALkA z*VfqHCvkS_0XalPcx`ABm@UqS&sV;r6o^08tAp^la5Oqi+$yM0#yrvJ5i_436F=jyMujZSHknz0;yX^EN7zVy#(nZAGd=0>{A@7>8# zmX{#SZ)Vn2YvJi1{Yw2a`s<&(C0$^Mlc56tT(U7#jPxyi>*xROd;jB?-#_HUKzD-! zF77)~6&tB66f66pfaWjo94$Fyc6V=z@J|WyYCFsrTr!}6x4$#IU5eq-i756Vm6qP9 zByB$k$S~B`+0@w7NpqraaGBOIfBOG`?|c-n^x#`fd||B<13&Q>SgFi0l%OnQ~m`BQ@&GBv${u9og0 zO)?zGQXC*qtKVb&h7!s#c4?M#v*K%^v%9UP7E%{y zcy!apvuO&DpQh8~uU6FstxdGOW+*f*N}YyZ+mxR#y!J0S=wE(o+o*dET2%?p82hSZ z&KS(v3xD=*dC))ox+lTKN40M3A5&WS|CQe!^xHS$pv>S5fB1v39|1{brfXEtx7Lb{ z8~VqfFQ%%4DZ*(DEkbzd^i!{)4Y08;0Y1Kdd)@4K^)UI0n-zU91Q=arjpZNmi!mwH1 zmsz?gVD6rR~_cp`M3p9>bXDt?MW}vJCzPr{N>LBIlH!t zzdh;iUpFJ?q(_E6{K4Ei-kWU)@hLEP3eYNcuDcleuFSAj6DmXw;a8IvK}Pcto118l z)zTJn?8Bmn_#7PGyh#DZLt}78n4H|;Z%(??W&~W_l+rzIJtF>*OwfFI%EVA_nINGG zytx7YV=d#tCN-OVRHbc2LNT>ueMb}v$Z#c|@r0gOM)mY4o%G>}TY|LBX!4NSlcK>#GV+6*DqE^!#+!cLW?}3*DG~I<-`Vn2c0972?EBV?HC~} z?^TmDO~G0LaLtTgpyYeX=TrjcY6#8b;wOhdX4O#Z=$J`_tQ4L5K}HpL@g(ANG= z)+0_$4&ex9TXg^0bx8hl^XbPP`OK$2@$pYQdWDME@~s=Uo_^8%OPI;KuYTv9?|lAv zVd2)RufFo~^R61z+2@}sp1!*slqLO(#p^GB<9pxx=I3v(hT&<&@Z#dj&%gNUE6;PU z_^MLm_2=ID-jCk?=1a#?#K;6z#^+vn@tLP?JFo*3?wfb++*(IBkCv+wQJCmhoBy#q z$tLHFpoaWJ5Vq>+g2|(K^aIlFpnG15U*sP5Qu;y{tg;!FrkjxH?&ERH%dfLz_3KXsmTzHQ06XkyOt*3L(sM7o@ce!%x}@{#cV7F}+dugBufFum zHV|ljHKM1lzVOoPufKxb0^QdWi4FYeJKuigx>S|%y59@0z4F3y&pmyEV$0EuJI_4x z^qzdjdre{+d0HAlJjU3RIzoI>kV+?T1;{|zWVXB#WqT;aMzM1@({lu4_7Hf15}sc; z6h_=wjo}IFU?9&Lm;$!caBRG`YN;=cpk+(3@MyagV(d~!WOlNq{Yj#~9|6!q^s0mc z4{VJ95iPG@ziw#L{7AAgZwc1P`_3))%6sYIR8J6i3hJPUVFNg9CWvH8LY=xDRI zF-Gvj$;U5r=uvM!`_wZpymq|4p1JJ3*S`D1?|$LNDjRs+u@RYEUORf_h2o`wBRNRFFv)&1nkwelHipWUV81dm&&FHRPde~_`$bczTusxewnV!zxMKT z&p!K%6|%?2H*Y`n)SVs4mSYa4o0;R7K*;o{WwuAn-c19=!ajZ7pjWF>bS9t>5d_kT zIE?I%<|A=9JlM%`xV>}@5Ut@4YLK}pthP_Ny?aN80Ke8yc=&Y3Vi$ z&}I|CsYf6F%!3a-^r=sN@_`3FeX72#r+j)Q(WEkS|fEsnB2IEnA3JPl3@m#4x2r6FU$2|mUv7#Ho_P9B8oxZ`$rby-cvTCOx zIrb3EqWNhp0xns9&ZZOoX4nehU;(^~0Bzo87!O2%=XQKr z{%Vzaw_R06y8&nV%24js&Q1Wad0L4^r83mpcIo637BSQ}G}N~Z2S~1)zpi>S(09PP z=-Z{=edpEZuCK3VG^DKPkoK(g$nqZ#s3af!B=~H){VMR(jCmFyT36bQiPCu>%~YjB zgXy*5y731x++9&%pHK2kN!~=d!$gFXFb#F5oMIpes!cq3{%oWm9s{t!YLOx4r#4Nk z@gHSf08;H)S*}~F2`l9so1gcNa(m>Q^K}c~5B@XJ+0fKPh=~G`a?+7 z-R&)noxQr;tVhB!ZS4b5(cX@Ra~JBH+gmmL`lq2#c<2k))AFKpXTn4+0V3FX<_j;~ z-riIOq@Gj&Ns@>MaPfX~DT=l#J1d)lT(1;CP6pcKZWK1ToW;WP+RQ1+Mp%H7*JD#< z#m($w9oIfo0@KZ9#*K}j8omfNv3QT;9R{(MX@H#MVGEW5$|IEbY+!xEyg48s#)7f3 z(2H1AtUg;}9KlL8C`;MnSWwaiRoVQ~RQI)(=9b2*EnVinIXP!sGjz@f9(tb|dP0lb1$I=Hn}HOgH!W)M$t)q7ocYYz*g1n&v!2nJ zWTAR!zcB%xik725Dk|3W;$D$Rf*+VboG^+YPJ+pC9jP9jSK3qj$82czikz|GZt6o_ zoqcG^J$Ba6F52j$BxHE#X~TX=q{KFTsBu9(FGj$^9MR77Xy2F%8A^c)*07}pQAC4_ z>YEVpijH`j<+rFQ?JE2Jiq8|q@_ZIAnkhRvmZiv*_qU&ZHDtU9<kY+d4j=CBiw4+~KzJKX1jj*UK$@{eD!cCDFx&BEhvP88s1dckE zfwaQ`>Gvf`jKkKzle#R-CsdWI49tnzf^qfY`SWL=JoWga4?p~ff1U`OBNZ}w;7}Ae z@{W)_Q;*!05~)$8k^qCW5mG--O)o|UmE>&tzDZ!S`A*DJ(95ws{V-Y~UeqR$*pva{ zq&$in6vGy!MeaL4U<-XOpe=*}Q*!2g7A)0NW9vW{6X@(O2Db+6ylZN% zzuMf^VN7RGqi6t)7QuvX*In)zn3<}Z{7I|}K^?@^+i|U-MGRF4v5j8gR%?zPLIR;;Q;qt5FQ7 zp77zOK>I9je7Fyz)FSuj(qkqb`h!|g& zVgMq;ZsM5~y-8K2H8UCT!hOuxT4#H6!^Lyw&e(?Z_@j@We9RjAE4b6CLbUA^ouTm& z_S0>>WAMUDdgUEiI3ll?O_-V^FbRc@ZU(G(pw$?zu?0i z5tP+FSq*Q@Pb@6MA1&ljlj|9bSGd+-rZV(i`MbKVrzb0B2hlPH$1Mb-RWHdiWmgQ= zh9`-7jG8$}I|QUDo$ni-tE;|=Ct^C@RWWEYsK07oTeCT>A&v8_1n}A9D%mFA-pD0F zcduM&>T^IFrcKorhsU>W-@Nnm?OV5tn{$W9H@9^)Qgw*DT)+Y)2Ma+fy=qfD)T8wQ^&*fr9>nfxK8V;C;HmRg7w??lMb?#uVZgWDe|GzE3X7V*G z|BVCv*3EkY{qjFSFG1h3LXR^=Bd1WR17;>#&a$yxx7lNH!A2ialEiWVPKLlIK1)JU zO7V>J1;VUblEMJV=`aifJjH_ZXOsXZANlNqpMLnWpL_i5WsBdPKdy$GO#ELYF?^fy zg7-%TGtMIgasKFj$^NLlcp@^{1bcV}Pv*nOirn_G+hY^v_2QvcHtgl3{T$h(5(f(p zn1LcGcqW7nMCV}~!Gd6URp6vkuLaoO(^rh#$z*zOG@$NSOLkuQECz^bC;qaQfgU4z z>G>;0(_NM@!$w>#G#Pep@7|JU6Sw zYfeUpf)9nOV9^_gTGk0Y`IKyE8uhH3%9rF$4gf3)YLBm+HQqCj>aC|dnOr#<`y+$Nne*ojAO`?>Tn33;5IM{fGI7tYjlt#?!; zSUB+-u@8|h+epNRasAiH{(e6b;4H%2<#b2J1p@FO6R&M;&4F;u1lZdex`e96QOg;8 z0+nW{JMbbjkFX|sY)V*qgKJdSWi&Zy6I)3sl*S+TFBf7kvO*Dpb~B+0t|EZLjwBP& zRREY8N0yUS#QV>hHR4Wu zzq>$?U3bM0pAmpu?f53u0;w9dExw5fOKgsu#@3d?c5xne$oYrGBlIz6;V;#0=Jw{f z8p1Xt5?b*4M@bp=_xH3lx3xC4HePJJM1Sbig@*bIPki!$kALD*pZ@HlCto>p<;ulN z^^MoAT)bF+<)YnCPhM$gY^ZOlztU#CCnTSkgG)KLq|v1qtzf*8tf6-R3)IuG77#d; zuKw3`kJ7A^WG$>ODSJ>gvhJ^|qrI_e8;xNRH1a8iMQuU(W5P&a@ z@|1pYk|KOHI*JN~Nd39E%L{e>KD-KyCMm4<&*yk~UsVIF{0$$?NpsN>8Qs?`{a|y! zoDj4Vx$G^JwCg2u0fto^DNhXvk+65ST(i9N%7xRX9)JAgqbHv@{p7ie^(~zin%5T4 zx7-wMBu>9rnp&P;-P$9m4}@J5GB?(W z&a@dGM{?Ov{*=eW5iqLz`)r4noLSM*-`6EyBWlpv(9ysW&)2uMHlBa@GY>rQsZTxl zxyR4cU%hzo(iK`ZSDUZZpL_h_(=4Fg#K6_|SbznVV{cn$0K$)F%A{sYiQZZ$i z(tUM$wCL&U=op!j39OzNL_qhjdW*s5^*`*eSRU&g7`MZsKcfp=J;SHofg>Ts!{8#; zy0I>El33#AiLz?~Owvqtkc5z#2my*L4t1q&ZdX78=zP+hU{5L@shELy0b7P{ zRno@YM1cio?gf>i%J&_r@Zt4mRyy4#mSUZP-3RoD_#;F*^il10tmoFI`b!rsH?(zi zT)TSd67MQO=hAvxo2JE>G9>nNScYA01(Cp&f$YO9#lNc+K zP7*C;AWQ1Hx*1}&XsPy*o0K6;NuT`=CSja&R;(D*h3%rzy8nv_J9AE-}}Sg|L7ll?9s@p_qX_$Ur-WQ1-;x zDJMNaw}B}Ngf!QJ7&{`=#hD?gMHgX)4zj=Z8ACcTNMO}i;b1ZPoe+BFw$>^GgOy3_ zf$)cz(7F!m@eyG5m?H%>8JFAw7Bd6gx)_1fpNElR#3>;ESp};4mV<6=3Ibn-2nCZ1 z#ehRWevo-GA~m1O7PwCH$F!c}Fi}At4RjHIrH0)*v_Eu*4kJBY^VgKKHdnB|ATIGf zDt_rSeW#vo2?4j-(%okr3iak|Eq%ONWooFi%~!ifpn7sB&vd+%Dt?*Y=(q9h^#6PM zd&U>moN^F=T3I|9-SlnBQwHe@WXJd7uRTAxi}cYy^RwC$mi_3&_O^5=R09rnw>C9* zwx0Xk1Ap|f2R`>z33fIKtRUFjiRnhi3Q6G6EfLB9)@!4F!7M- z6aP9Yqe!!HM7KeCR1^HBoRq8X)jSG=sNy?eN|={~8Ai zkl0wp>!Jy|MLBuL20URc0YIP)|0g3mnhpFGszfpU=h+^SmQzq72O2O^#%-ul$`t`X zU@lkyK9R`4EriyKs)*j3y30~}8aeT663q0_g~R5i#!hpC{oRK3$igA~k>R#|Z>Vli zvkgnCvuhC3z*d*L@L1-Eh>X`X($&)1JF3a&S$ZuKSn59+Na9iQnYb&Q!IXrlL-<`g zV2lWHk|PtA38?9?Mf|3$5|D!}R)#vRo_XTrqq2aDO#of#+8}_p{>g_v@!7{7KYjlE zYD*7pR{HQ6YXdmMh%`|kFiNV)y6FuK8%}F^L0oVV@n79+y}$^S%Jjfjb+bKDRDaz= zo=WnU7y?4jRMwRk^EnizGH;C13i)5-UwE3Hm6Tiz8$k53?-J8s+&JpwiB}^A#*Cy` z=r^t`dVtck&)K)5^J1*3G)61tONa^ro22Sdj4g>Qw{IL3yLU+tiroi$p=(AJc5ic? z06eddv3U*QGMkebCJ=;M>9eNoxQ(7f#ZO2(_|Elk$V}qOiQ35hN7#2X5fr%@)@LtW-Kc_G*UL!|EYubQdc7F9 z%KjpQM{^VE?P0JlvZjcbBUP;g{ZXH??*LTZhf+eW!}%4}zfiX&shKj6B2i$5ZmlAP zjbxBxu`=A%aQ5`c#~ym*iObgzxJaxGj}G)Up8f16A31gE%=xoTord!=2*#PYau^Xg zSDBFpBs9CI?cre0%n@Go!VE8+n_P7N<0E-w!DkGLBiqb)tP84HZw!`TH!WXYOdL8x zEOeH@5L}YjKxHO#Ns>w0)I=t{Sj@>N1TUcer-jTA$3`-FYA@F0OZL(98ocPBAZ~_v z7)&rjv21H|@5nAu@PkdU=B+YoOI)&Cl1){X%hoe4w!ckTbRP=iP!-|Gq-AlSCF6~Q zYXioXjry&1?Vao%Ew^}sA%>2(C;yBF z>XOeUpsDPL=#+kd2Q78>Q>D}0n4gIaWz{ElVuiFoFg#L>4e(2F{bFiEtqw;bbr4PI zAUM-xVKWbq&BE+rQXrT*VWOR`lVR7yOZ|`Uv1N2z>QnN}?lx_?=4)4*+Ibi)5#GqU zMF6kfnMSjlB0R81ukB!$7B_aUU*Frlafn=79S3FHW6s;(RSdr%10g18g%+z7!?>w2 z0u}M zaoaF3B{-^}T&KJJS_>WhwvK+?$xJ%vtY+#Wqqh)1{BsvkS(jv=w_n{Rw|=;H`v_IZ z{O8!pcvZNVhPnH`E`Pa|{8`FK3`@v~vIQ za9Y#l%8+9JZ};S*h!sd3jKhT1G}^3{5Q(OuG5r8?+EKsv!ArOzSQM{K7Y?U5ltb6K zX67sIr)H-JWS095=o7sJ_#YS&tKnzVpOTz_4{h{g65#=gq?FIBi;)W=rf}ssGp&9< z&)1cej6osJN9X~(>*wfK%r9}#6gK!P=Hh*^!oW1)5Bj&p#Gaw<997 z3RNnXv4tR3V_NvCk9nJfV>*2YqtrEm;bQ0!dSS_LWQgpyC$9E(rc+9_e7kpmgE z@D+VBD7t?PRQ%Y{Nt3ZqrzDZgSILp}@M#3k3=qdKY#HD?S!N#8wCYvrsX%4jOLOK+ z2M1|%xfW4d9Z+Cs`i7t@dnkzqKA?N-5_xvQbhQG*Rf~12ScgZ`7{X!4`4}k%SHTI` zI2;hb9ee_?lP^Qm)J=kW-TdEAGt2}DiJ0zE8D?rer-wPGrU$JjGWV;072lb**oy(su7jD`^))SMQO8_id~ zBmGdq!9QV`@=*Q|@re1;1YEEJczP&>N>#2$N<3Tw1TTqBRJ2lwyXfnv=ROL{m7G+` zjS1BlJKnu^TO@R4J2K0OVx(fqTVVw>o5i0rsx`gI@wo>2Z=qWB8R<{aUmYbvpf@4v zYF%hV&n-l4OXBB^mu?`OmW5|=LwGgx5Yt4Sn1nk?lopTkheA-M2~RXl9P`*2H>KGE z!k+sXgS_Xsm@eC3A?EgGYAJ!#8-PctxcECpSjd!ZG18Eh76{y#j^U=OK01=I*^Og=QSTbT6VL9ChGObMJ}5r zy);v#@xoFSh9DoCoV>3l4@7DPglAYLr66J{Bq7r(x>Z7jH!U0Ry8RuP>UF(e>X&8r zo6?@QP6;y$2fn(Z9vSWJ8)J-WG<)a6TLuYj1e^^2h&vJ~^^O9y#xWHuUteCWxp&Ng zvEa020~14(bK4}PbLc_I|9yc6$s-N%P+hL<46V=Y;N?_~G7zZ%%IS}ZCzo>FRxTSW zNP^%JxRjb#8G#%~+n;%w*hgTPUsQSt*dop9+T29N9H(-FPf6cEFwi4#x7b7?yE>K` zkwrU9&L2iyDvc61)uv%1(pPoq?Cfe8mfX4(4zeGcRx@^fOu%#RBE6|3*J+SN?`=zY z@5yRqUV#3i^u~zA=2QmrpjIgCvYhybI!}s8&{#)R6p$d9*BUN2l2%BZC)U^l1sLdS zzG{<%po>9*)mtMa=P;#}7zp|bx`LjSAva_^S`fnmHcF@>F5f38X|yzYQvnutU;jd156;QW?oJ_7rR?=FY!J(3>Casf0 z0J~6Km`tiYP6D^VLgUHl#r0xqOoPI}W84m4oo={HNNG}RTnBxDq)@Gxyy#3UI!E zek4?v2nb*cE--*Mggw`?Hb+N`N=|0|5;6z-TdskQ2ZnpDp3~f{C(nQ3;)N?sYM6yh zOdDbY85~*U+zL4=!ovs|@ub)$sZk7q_O1zvO8Zq}9$RYyP_3BSq9l&Mk=&q~r2?Qw zDgw@qe7V5{%O%D}>c;+VbXMUh(BZ55))Zm9rbkV;k*{LfVuj$CGq(O|Gz<;_#uiE( zrc@NGtHD>p(9SVt6q}7L%z|!P`avwqOW5r-Jmsw+JHvVMIn#0h9B{2uYf_Q}2+Hw1 z`K7vQB^-hb_=9`YFF1Y0_(2j*X3bebBT!iq$I}x@;T!&2+1v*4m~8HrhuDPUCS>d= zA3)#~U6h=D!0$)`;_hgKi% zNd7cRrO@+pe;n$^e>L`?92?#Tg z2&meS44~W|E<;kKgp?x2ye_II|$3|_o&{?wKBZtGBt8r!-xJVZT5 zq~k)kBu|kq<&`5WZuoSay!AFh*)HXa+J|OOeB1p3zBGc%4b8nHj37ycC>u8*twca`<=igiq&T`+?Ao0!k+TQF=TpCE%It22ppzY7 zaSSOS7w0R?2c9~i68lKQjGAist}7)A}ov02DuK8k4qca zGJ41-1{KJ#FoWV$sgdP=uST2dx5wdXdtWi|%wVUCvz@AcKi#_SzEPyQM5xuJ^@AJFJYzH|KU;pn{CxRY zYYTPD@0gL|h%9XkYg~lj$yg2?BPE$Q;`IIAi9yoR$~0MxZJ)y<{r&Q8C0o$HkOu*S zcn2;Dh-ux;~@-#7$6~KqzCcEKNN&Bs7H}CB(OpRZ? zXk%s@#cX(yQ=~5|?}fF(MCICEg6iXFtyt<_savy=!P=MQZeveF(bo}7p=CSZe8ERi z!tQl-IfwvEA$!0F61ZdZS>%w6E;PlhVr@V_iMicRD0Fur3Sr`h9ID0wf~_cRbw_0$ zNkub~xxRM)dau4~F@NmCF&cFPAst<`=$VS4#=&(aL;~8FJ^dtvj2B(IT3jBek5J)g z(YGnI;y~+rhvemxxQiT0LO^sxsTa?FXy!z2Rz6O6CwxVEZ<6+b_J)gCCv_&-LRX>$ zVF*>G4G`dY75%H**YCXW!gal$)H4Z(t@{x>f%&I=78(2jEUtmi4>j1@Zt1 z=em*Q#6Yolw;b;n_mp-qg*2HDpf-%)E+|R%T8O)pD%YjDt;dryM5Juo&$ZN1is+@(&rjz z;-EM@@W&<{*{*Jrojym3x*^?jjHvV|w7DJn!gOWMB=YO~hx?%`s6SE-N|Jr9B$7gS zv;$N#QfJA0*39rcxOaAmiG*y#pySR)vDxF=Z%!l=LW$+VHpM+7Q6LSbMA8{9IyPR~ zfJ2Q9ph^Ncx9heBcfyF-B4+qw1Ko*n{ElGj_XIfgnZV9v+%rj@WYDCvIegVxPfx3n zs>YUfCSV&ta=ry1eM}(6V?c+<2i?rf_@Hh`Kg+SPe%6F5Inf8YoKpQ&7a6E7+E8Df zlX9y{R7jsWaBERnabHx?9=y~%?NTMa>523!g~FopRjw^6lYV`k8rNd&x`HVP4A_dG zhl)_d6f;0FwMt*F)kXTkB1guxWK?sTN4KAU<@sB?9>L!jbuoFt;=FCQga$SbH5CO! z)n~L3gXf9&9x)Dz1xY$ORhq{gda(#}te3VL_r6%S@Oo;(Qqdq@l`jPWs~XlU%V7MDKPjnOV1O6l zk^kS~Go<3%xcg~oTklr4mTf}v1P+nE)D@z}gvy-N1Ti^ArW0y&@-{PC=~lX1S5wv( zaHHgi0m5-tm}bQHbq-_!2xa{}y=wJvsIOi;U7XxGedg?i%U3R6yxiE(IY^veav~p% z{rb253?a9&A2e~|uQ;|9$gEqyktWY_Ibs0A{bQz{7uP{`85#@gKv%>!k$&G!v91$9 zTteZJ2~RTnVc`2j)}HXeqEz;+B!PxUf7kV&0pM&dLCus6P#S#+2?P&I8|8mD`$Va3Sg5_FnLVwMh26kpv66 z2kJ)t7%?Af%3d<9RK@ySjq)xq7G^@cT>E$=-9!#YKxd2Tos4!%4s0?^JFpDkNzCqF z8j+I42?HhzuE0@e0Li`GG8?g>GLfnlE8~d~a{09_EA9@8-Pt3&R|k1~`^L>f+70|+ zBsCU(YXGS*-y#aOn#y0K?2-6APy8q``2(z9Ylf6P8Xqac7X&!J8}5phrk zjG6?Y#t1V-)zVZ?p1L@{bLnbhb2u}NZ5@M?nhML=H{+dwYDBDL*qTqs;E7XRnLJt=t=OhYB-ub+Q3S!aUXVf(hHhBbhF+DZEad<2DpSr@n$v5(w zsVXw0NT*Frw6Y9X2UV>*(;(t1D_$TzL-mkl15az%M;zn@Aj>( zu1h96WiuQ5N47}q?j7$P5Qf;heur!m3)S3MuUoqr1JI~3(G*}5O2XP8qLzcEzJY@u zka)U*kSAXILb3D%H@{cs}J z47^(#)ZoDA*g(;J)YV3ixkW1cb~zr|y)R4>P24qDRfPxS2)^ zKNuTW-aL?PxZzGS<4KtW5$%hqW8_uDvoW}a4#-f~<#TN`YGLDk!K8Cfek3>o$dLi> zudNCQrb+~WTzoQn>uAgpi{K0yz@(p?SxwBcxTXv!z(|F_4U|CyBNXyIqOS+KkY)c= zb(-`CWy}^O;4U7Q;WkIJc5wa1{_!DI<|;;<6=+~<b$WB2Zjf{31ZEfrruSS#4G~e}GH)zM~9vmO;9@!F!j|v5lF|wcZL$)>} z>TkynmSCLr2C|i8xY(I9^PAKSNTgv-AZCwho3L5T57&qs8zh{l)Zr2)zX26DH5ZMW zHVgG|c62D*+?fzt5(YwdM?zE?&I5v#|J~I(AQ}J9It7RDW)9SD*>yZyOb-i^>U*zH zH+7g`GccBp9wNeE)DTW*R)fZ zXwjXLwkM*eK=Z2@n@9&$TE`FuQ0apwA6vJA*F8yrDi4G=Dh$g^Zhm^WyQQJMe`diE zf))3VZ{0Y~*&iLz4{`a`lj?B{x2$sXdOq0vtITXhZ2?TBEaTQE=hGK+_?`iHk`bjH zoZI3JN!^M2fiR?KpCz-y&lrSVHB08gv~s@!C=znqy~q3FwO}c!cjtF zOsMzM@La&P+g!z3ET*4{jtc4rfl|MD+HT4U3Y+3TbhJ#SO+|CaGD!-5StJ}$hn3~~ zSEporF+(LC>_2r|F@2P&7RaGbV6?34s~Wb``J#}GwoN`%m1GMW$D$j%`+fmqdmOi&mi>kDyg0cVIwoT*?Gy_r7OpdVxzPZlMm7-_z|vI!ZtQh`48|?dN4R z&PYio0u8Y@i2kJ->bYA|!yQQ?tc-)oO;J}Y(hLA@%6QX>T`n3zuqkAR6S zAa{s?zCV&Ihj#i+Rkb~rH8SreKMZg#_*kXM2lzwC1rmP$=hwjCWQ=kGS|2lkHXKHo z1gI$lNsrwHOwHjRX#h(A8*}{Xe{(}e9C{w8TCUu@qiwuyrs0OtFTeOYXheQmGxpVZ z@zGB{@X?Qb_F{`UOvDo@gL`evLZ>V}aGOi&49Y@#_{G(~2m8rYoWpPjgR!S|XbxeFhm7h0z(Iq~i0jd~qd0utJ(3 zYTwUfU!EUP&y5oZZxg0g$lgrWO|6!!{bVs7mO*iM&r}#+69I@^L7god&0kS93+G742>_r^(Ds$pO%1WW#z9l$p#3!(YrYGEae!oPXxpOl_6GR{( z>gEQ7A)YZ#Z+lzyv{5YK=t!0Th|b7VDV0hqix#zX2{OyxmFyZDhr6pwGjl9p2_a+v zbkBDrh|x!l^t8J<7-khyWgv6NC$O@)DcYBMn*z7wo#}J+Y_+rij8raOsKtZ|`+aTA zMK@3y6J5U!Rgonk^wgrk$-tL!V-J(-^8@V8`_yzC1*x8X=%G)1>d}VIEKLkg+T7yA z?w&R;#At8NG|Wz&*j^$IQhSxf>PWNYIVW4E6)q^SQa^!TO|e_-y{1$+wtMzy4=u<< z4aesxW8DOW_W*@F6+tOenzkTx0394YoqTJ?;^1OqJd|r*AbD&pi^5k=JQr_)QY(3q z$;pyO!dV)ZPU?A^8SC{oya-J&NKKM^LU$_Zp<=KnOynB^t#9B|Wz@bSzJiPPr1k&V zL!M6N4g;Htrr7zJWL#P;NmS-Y*=_pIikVS}KH62NryikXORZ#xX+gT`mI@x@&`+z; zygifp8Kp>vRei^SF5Z0gb^pf(Lh|+nDMOjHX|0#m%mcje!iz7ye1|nMPxQOxWf9af z8YBYLSJ3P28&~Atotcs23C?Nhg5=9BpE4wZyf450#v895V~+z%e5x{SD!q{9Jm?9L z9ZZAJ+3Qavj@k!9F{b{)*(XmwdG3kFo;da7=~JiAUc7Yi^3|)CE?;eE#<(*HEQy9g zYQIPy{#AOpHaF67`P4(7_~=JI_TZ@t=T2QR0ck=TUcvSh3JMY}2xHIW$e2$cqv!;c zy#$XK`%5hbTAi#6iOlJ8&}Wu&3-KB9m5i_Gv+Crs0~PSN{Rd+wW}G|TE{WKndZKKI zHZ#3$Ofh!X?8rcVGG5aayjM~O{>!Snz-$amk^q^L;{}?OVu@nK&OXtW`` zWo+MpO3i>V+|2U?c6m`&Fq<`#Oihy_R`A+QPp@_dCX`&z?Jfxv9CWv%9mk#SBv))hah=+CXb6Uc^gB$~DXk zTs!}{PyNr2Jn-1qwKn zym|W<*?bineb8l8aq|EY5!nIR0M&d^`&XAtPHg5ep2mel%U}a5As;#T9dd43!a;D* z8KF>uObN+HI=NC-jbY%Ya0W<7c{X;|%y6@NzjtPO(Qb4fse(QdnrYJWkolG>%geXl z_`(;y^o8r&L=+=P{)|r=->yv&(-kk~=AY*YUU>?jz`v}{uddbOknP4 zc2-fs5d^gbG0v5OhMw#4_RDX4{`0TjSW=zqM~AOgto<)E1q@PeHusVtFoxi@LW;^? z03;52?ec|lXHK6v`RK_fGunQwi{G)DPvJv2024aXu$#dzTN1BL|sk~EpSOw zg}9?UOF3RdZ$_YxtOA5!X0~qrdBqot2)gX3RQ^l@f+mt+YQkbnvJK$_^@4;vOKt)< zoIGZ%7=OhwZ73%%FJ%b8q@2howHnV|gMcvU%$s!x$np!eap09IWT_zGc{{B;7 z8da|^BVnyhMB0LHO3Cl(-2cH#zhIND0rbvCotx-YG7b8?+MUyBziIa@D)iiK$ElZB zom<~XRfpr8 z+!p;KkMdsroz{!f8#gpOdg87=EJld@Mdv#U1;qj)z|<$GB6M^DRRobZg)ABmeMHe8 zQKiU5j!$GzMBbxT_go=kN@o|-EsI=4rr+3$uq!q|gLB=Dj;jc9R%Py}FB^9>r4@Y~ zItw$MBBV%EeFCM9q9QR@_@7LDAw9s%j+fF^;@|6qc&vZ=sDAT=d z&CN8xjhcwae$ME~P-jd1#k1$moO<+gkDa|*Z@X1%hk0jXUxMIvU~^Bg;$%?my+6J{ z5I9f?i39|05BB%#wq`WP7&s%38EC?WkIA>vwQ+*j)iR2lP^!D>k{k9VRxhR1O^*R2 zE+rDrX(icpLp4|6%||?QgY# zwG8A+o?O;#YwC^oGoKYF8=^?KuF7;pR4l>Ab==s0M9;+20JPFAioX$&VtCQfLIm=f z0crVO$Vz$s&qLjzc%ls!VZc?W`Mvvp@BIHsLK?tE7?O_8AvPMXwnVBte?CHC5oiw} z86wJR?tgsD+WL#m|K-2LqY^yUlk94YNq78mwFx*!m(H%S=me%4Tg^mJ&Yz>Fc-e6I z{NKhs^T{zo~@w7f?dcVD`gC-C!7rK)aeOOcH>V=C>KJmGSKKsOlOJ^>k zXUckWpNfU=FkM&APIVV!S+qDfCJ%^)yB{vWfx<#U!X+?p2gG3FL^E`n$1DA1v%F<4 zvWFet4<>^K&z#t9>eMx3B7xd9S!MhoOZX=nPCf5Lsbcn9JOsa@n;X~XR|tEnkY&RV zR7d#e!Sp4mn6!1NKw&I&=UjMIg2uH`$?#a!s68glMhCj=s4YeZAS6P>?|#txer>95 z#!kzr#ee(V4^*7hx|yCSjO=1Iw5F6+SY32~Qqz?05czv9E0bz=+;IvvXXjK+b;^XX zJ&VbHqu*<*?)*RR-m+SOw)mDYUUF_!IYcyYyS+z-cOa4u5JNiadti=!!NS_o2)@o! z@7tmEV@~)RAFj65#$(UOw^<}dxA`Bx^TE^?2aJY*Vytes zeEwqPYybMYAMF0Gzxn3Pq0a7}EQ>_j>N2Q}wXM$TY}UhX?P}@h>Fn<=dY|Lm6=23) zd8L7V*lsHZ19@JVIj}Vpa$Ox>N|VfO4+($Q+~#)O#(?+k;ENR=lnhaNC9nsXKqOWi zVwvx6clHyUmd$XBrkwx3gAOO$K?l(TjE%bCq3~(I%W}R-YEjYcl2sGel@@Vr;+xi* zRgJM-)foSh2QtQ0ASLFIL3~5w8Q%j*`8gMOv6h3LyLZqt%MLod*o^7RD#N-<<9dCm zSe=sVqSlMvWgkYhBKq9?t7acfM z6gH*@EtLfrt+BroY^MuDAxI&OKd`j5HsN|d>!=6*DpZM`VtZKPi5uC zR!xjO?`nu=lF02G^&g`Jnmo)z?bQnxCcf&ZuRH2n!`;0WqK?64Fx=>OL|0_6yOk7R zd!G)P>2dFi=7?6@cU;8ub*)M4GSftmU9PMe0Q^FKbqM_X=;qBE{y9Free>4!WPBiU zfnLCpVuLbafoX*!Y?t-az2V>So*~ygoCcTHMFNM#aj;T6SRt58H2_k5M_Dd%%j{Lp zWjRV7PGI9_VVbUf(lrS(@*H}7&RS&mEA_?H{t0uIpLzz$Liz$-zKO$uuFO||_k*$b zwdj35+dXruH&dmDeCEA4Z4?Jm<0uzCuXI!aND6leh$FDd<(A%;7~m#MWqx>*N`rBTDsc}`A8+}@>mC06Ic8lp+h?T88}vkDoD21CgAxw>*>Ej z<*@umQ#+(voPzRNKPv!80>{66{|CL#&Cc>jyyfP<=T@7o?Gw)tuP&WAJNDLZj{4?c zXLlFK70U~s35V7{%%Sn}mBwow%^jpUj1n2Ah&+P11hgfaC(-pW8n{Q1h{5e?KFI5f z?OqD7#m!23Mv{Ai`3%5_)!Jglz|=9p)iU`V7p$a9tA_ucI6x`oYwo?Hit2yR`do>%@|Aw3F;4V~KG!oh`kxFKFjd;>R5tstd4hC9CgfE% zF!Cv@8AlJxA?2;{huYdcDVXK?IPd8{Pih!WEjTUzPLeS4{8q$RuBdzM6nGD0mF^v^T5dmGV>q zk&K-DfXc2nTU*T4O4G%|%ql}S5rC+?%-?3_DZI!yN<-9&=x#c^riwrZddf~G?lo|X zNtz6perV!E?P_!Ozmy5*y^1qZOUOdw$27m|)<+cCx!Epe- zB){y-kx6ysqsict6lhcV#rcE$y_OxctMA>A*cz6OV{;CAChV3r+}ir#b_gKP<%s{nHCF_wf~bNi-8Za- z8F~MiV8Vbad(1~)l%x2RvX(a-@jv1L_ZpEWsMFV*Bi{V=u}Sp`zo-unX)!vO)TdOq zG6+Tha-?pkN_e8&-vROD%W*H%@kl2%;?pI}8F>rIsNx_cS+RCNYb(v{dk_r@eyC%T zQsf)87jz}-7FJ;8RlNM&hPGbhL}hjG$E}@APNeOf)=9bhK?;xFZ1be*wFKg@?{UJ) zNeo9U&;RWaqc}&R5jvOIRnGu28-NZCBS)Nqw=Z-$)!&E#ept~2nM6nYl3%7OU6^;o zJ?VDsIbEiAdmgDyIlSB-!&g*=F3z~cTcsatICo^nGreNoG)&`%{eFFA`>70<|I-g#UxUJ zRJ2G122nOMg*(4>Ajwrvl@%z2bI*R?9oAlZ?Hv89ntU~mjE7|;lwa#G!?HuW-vJ(B zoeA1P?v_OxLg$#+OQ2uc0xK$HO<(;f^|)GM71|=|z!uG0ysEQ}8)R|0?=a_>dWNL) zxNfnF+v|bvf%NA1X-r&t(V#LzR}`v+=qK!`0p|jtb2ypcokC`(Um9B|AcXqKn3Uka z9YPd=O7moh)xWA9SyxGST93&`6gQ*70365-I2`FW^k14YKwBl!1+p((C>CaoD%YPS zmi0gGerw;*6j`dAG9@#JM<&~D#!;z$YJ^bb1FiiH8ueqLoj3vNh}tAq0vCjoz`Gzn9|U?N@3-{ zZIY=Jy|NiLwPlF^-lyv{<4|ce4ZOhx#=$U&`sJ^C{xZpkV|*N@a_rjQ_r8C*OcF|9 zq^`s@!Do{&1-}ZQ2+}kuHT)0&BZ=~mRZB-iqMSe*OgbPSG&weg#1K+>%p@?w>$Z*# z?tcBPeFp~7$YCs&_K27Z85FOfeUSwdyaFK~rM*K50okn?gpECy_YY6`!@$K9-xL%u zLjaKrH8M$#g%UCHEdD`i&N>wsRMMO171K)>Uy{3z771aB=mhgW!W?Itf&`3A--wmT z*h#yXFck8lr4vM}DPPb&H9F>wa)s}RK?9qi_4TiNcAZe8fMl0Y@-!0!l(-xdDtdQ> zN*u3GYKMVooR|il6GV^%%Hj9Ka_M4{IT^AO4gY77ul1(CgL7yam~g@IFv3-u1-$o| zM&8IWfbP9ZIV^u z>=`xv3Jcc|-1V?n$?H#F|NGuYFW2}_yn)cP=%bqB5vC`IH54H1iq=UQ5yA;6d@|cE zK{Te=Z5AdYImk|SXVDNOO~u`0&=H#$fF3?Z)nzy;I7=#QI38qT=CWJmN$dn+APN!* zKRy_NIg{wCzqM!Iz?dx|{x}b{bFkW-Y_G$!h>0#q|OmaL-MNW|DI_ zeUR-kDMCFRTvQa1DZ=BKCMN%e4<(`kFe-_+njN7aC8A~0Ivrs)*Wype#7Hm!`qgGJ zv88Mjn!Hoc5B`!STfh=JTQujxM0QGVGPh}zZxbnAcmBzoA{A>66qyqRrgISwqRLV9 z%ma=F@2izal#Ad@V1EQ{{6HgIAbbmdq7OvETF1Eq>+!i^V9P&&wp-Us6RSAY@B@F< zM8(b}UK5uam#9Fz$y-s%kTy+R1p;^OT;i*L-+QV{te*cgYq2sJX2e7+7v)@OrX6Y@ zR}3 z0FF%C4{sxXNAW{0NjXN`MpTUH1CVIGgu!`sG9mV$cc$A+uuTI;K-wG|95?`v_YZHp z{>MM<#l%mWA^#LxCz{WG-MfTg$a}bf{L*CH%X1a~eD>A7mqsuDa*f|kwoBKTNm-8#y&c_Yatzk92m3=VrX4(fox*p? z)o4V(ZbR>Fig_+1%nl5UB8Q{A8^zXx2ll?H4%@z=@o}0KoM8tC8GSrvkckSsIL@EQ zN0xC%zsH6;vk~pv9vK`MWCv~#+6bWtu!=V zw<^2`SP_rOMdD+_rXj=#T7>FBUQa#)dLf1=6j<>{im@PJW_)OPm}lSp)|-E_?jMk; zXX7*I1^T;~`fciZ%6)u^B1O_q!f$SA;}Chm`XD}^fO#4v)6s>kBS*KEQ)C4=UG90% zF*Iax*i`*`orX97E~A^7Uz(eaN#~HJ7U>0DRi03>2$(2cFcVw=SJsl_j;XyhxFEJB z-aOissXqZ1LF64`S_f+0?83sIO;Z!9N$yi@KK312R=N2E4tcA9!%91l`6<^htcDOSF4<)G5= ztyStV`h<8`6O$9;j!+Qw1Mpc!M~9i-eDkf{dvy7Q*3 zSD~<%Sv_+}*P3Ig3z1!xt*?l~JwuV^o%`9=53u$2%fk}WFjd4-$&eoz5soNbU@i)p zNuuJg{=q-31_Ze(6cCCjurNE22r=6+Wp&bN(q?43X2Zh+``&sB&Fzs%3zvYVga>U7 zR@QQ}cu8`_ga}d^`;-O>sot#SR)l09KF_fD*xce%Q-Lhd@V7XAU=Sc(6d7v&smCmG znVOy^S_*A~o6Vkw$O4s9l*OuE#HLu#1yBb?x>Sw>Zqjtnlok34VpW8#DB7p+0g*9tuuha= znDLRp5w?6APM^a+gVG`0@C7cm>o%ztZH)e$OZ8q<>sGx_+Ne;|DvA=Qy{Fnj*61(3 z|C^rg!Ca|AV#TYu`C4!0w0+F(kLEj<{`}WN_^)J`DPky#c{}7CG=iF5ZIfr?*5NSXNv2^n+ zYm8KHvmRb10ysH3HRl&GAjeX)`jP$vLx(&1$#M8ycvt})fT&DV-!_bDu z#%=7mW6#c{$PdIOBs*agQ1$?wq!Lii1wIq=kZS@Xz&I%=k(qL%l7-~dDJISAy9U)= z;GcV1eGAeA;u?paoB^|nG;R{Cjlu^6IO->HFgOSt9-83loYRqxL;MS>uqC?4(lezx z3--91v^t8pNIuUd)yRx=obVN-5l-?)`{;qaZ|ysH@W4TAeUpVjBqASir^5*{XSy#9 zx)4Qbe_k}K5mE{65G+8RisutYH-HunmmDmL%Niyz0ciCFP;;cP09J=cJc6>q|QSPsK$RYsC zk_pq)3zY0UP0}>=Y!}^=YNK{jrk=0X>2#4&>r$WfT75H_>im_j9wVpE6@!A>Uzx^T zF;5Hfo`-}fSyCtXDxm~v6xs(EfZ336no$%~^NoWihoQ9rFip~7;}OZ$$k$YSV`&-( zG~ggo@@^j1TGfv5ph^I^-q~(u67$K$C3Sg~X z5&5MW3GfMkDXH$|DTR6gT?@HadJSD15s||Snfyi6bZYhhK$iT9YBW_LH;Sn zw1-tFg7d~lM*2ssLxb4)`LxYu8Fs$-O>g^;>}q3|ZaC3gKD2dsF2{aAEuK+=ClH5Q z1@PTiS*;2IB{?L$z97v z#REARPRUr_ME#lx@6d!z8)a>YrNi?y#Fhkj-lB=1mRHa{Hf%-vEA-)x`}T?OtPI?H zLO2xFO8STXp5-qzwqBi3qN5Dvdsqxl30sWRGT>i2RM%W76FWd~RFaxdAXSTVA+Qm7 z0-uiKh&#c4)H*XPSS1r5r*a;jIIw#U%P$TLT5Pl{dG~O@@JducWb(8V$m}@C6zx<_ z&_>in2cwK81DpXkn4EZ39lOFf@om6q_?=Ye%m!&;EcXxGDW&wx`x;wdbuOa{Lr7ZBl4sM z{3I=5Z2ltR@;E2`*!%@=evGAD+4AD%(No7)>l`T(CXIixILU_9z zG6~QnAvw{6H`_h5C@x}JBvvqNY7ZPE`4A%w925B>B9xi-au^8ho@}jCa5KbdOoYdv zdwHEUZ}!mEQln`0pvlB>EULgmwh7+$O!z00`vut~O_QsYKrbmTlmE59=X-UMWm%lf zWa3eOZ{}$j1MxOeh3a~Xbd1g}VyhO$6CVnZXQrALEDCg9BbR(A!WQ73ybFpe(})Y( z0nG-r|77pJ1LWltbkrfrS*5Fk_cEEF93%1^WS)5?8uVeF0Cou8T{C7${!>O{sENLH z&6v_5mbf_-X0?^oaY{%_Gc6n#cazn;F+Ye%tSp#MjUn7RSq0mo$mJLg ztIu&kDA@7))6Q{YBFo)iD2#g69VRq#+pV_A(cz)Np`pQn9eX<+6F%(y-Qduu6@Ol? zFKr$@EqhCppiZwsqdMxA!zswN4;`u_Yk>zMiQsv#tvnNZkUZ(Rm2OHWmV3`1k=k+v z5G3Jmq-vT;ApPY(b8iiwA5_Ydlf4fP3>hO~wqMfjar&}{jx00&v|_fcXz}x@;jTC> zAhU6;Nq0Qyu+X7DWs$j0g4xjOsN{Yz_o*t_3%FIxyx}lWV3k0atE@HE10V80fk5RnrKe%V_erRp#@bVL(3Gk8WQUm)@ z0;Au76Vy9GiW0IFJXpe50FTJ4V9Jl*C!e0~TbEoX88e)7_9)g`Im%f0d!Ti!rclD8 zb>o7fNg?S2;oJbgfH3?9d}I*^*jh-sOWY?IGVeP@R_krMIY8j-0Mh{!a(P0W5IHA* zAbNU$K{+YLp_k!)t!MqqGo$A`Pv_2l$^d%&fi3fcg98T#;ur6{q*eX+dXpI?HS5&T za6DbFpLt^R>{p^;-o5L|l88%dul4L_r`J>(J-=RrgC?3BpvFB=Ghg?2JwOM^`}^m< zeA(X%ZQomP4@2LYfb!xfeD=!dIi9^&SFDbF0Pw|&0OxagBA``qex>c{A(U0@?7^Td z={ZcLr|mCm{D&V;aiP6;upT}CLXMw(>oG~1$=KnQfen$gD)3QMK@@`V2!5lkxo(dd z!YvkdW+HaH_Sn!QoOe{j4h$ip&E3-DOfFyi*aOa|n(rf4mu2ot(Uqmuo~ z6$Hs&*?P^&VZ^3`(I{&3@xF}y$&3w-P@>qLudwR?5MIpgkP(=prsDt#GeqANI7sO$ z#XiNHQZ%eyK>8N%KtDPG;^?5$i`ccVE%iNa5H(jo+!7g>XP_5i8hBc>JWO?}62riM zeU5=IK)iCC{@K9eC0csz)x<@aRs%5s7(9F@ob}YRtCNrkC7?4o*n{DPHke~ zbCLD0c-=2Pp#Oycn2#UryhoR6W(5Pw-*`Qdg+H4HzP}o#4EG?_=?m1ql!2dL>#pkp ze;oB&-I|>MI)&j~$?f!=srRXq;*?mPEfN#T%*4@sN*Nr0c->xr_@t5m60va@ux$?~`S@(l^nC9SZ#VY+@$Ft4mj&);V z8DL$V?scsDc63eFeSV{cTqEoxC)T~s_(p6M>+&H39#?37=lb4X@{UeeoKui@g7q(U ztUG76oNvtp9TZwvkncp@IBmA84gq^h)stq1?HKL(ge{d`|Jl0UyG(t-KFaI*%Djvs zgUM-0F1>|HQb5KZ2Y$=7PD0Ob65@XwD zdvSSTHiN7_IiNbvcs9^e8pZ*3Zco!bw0`$Km-p*JD44m+<-Pn3W|7jr?aXg`-pA)h z4(!`IX0_hv@7bSj+~N(|uchRs<(Kgvhk4_d-+unT_1w41^!bS#y0b8oob$iyaoFhp zq&|BAYW3qc1)7F7%jnqL_rL1dU)^p#=Ca;!C<4XtNzCM^Nlib@`)nn|S`GMZ^2Ueo zk{Vq20CPTCA^CE?nwNGPDHIPA+1{inz)%ArS(-g$t48`;G9Zxw_#fv2k`GKx$(8^q zMBHN|`}a?fLfb%e=n^ zv=P{3P~t^k8G}+3PWy9Typ%z|tJ)NZthU8RGU%ssGUx~3iUABtgoa!|8ftpqW!TJ8 zl7TPa*(9sPTqPdB_QPRiSFcv*+s*!l?D-+~lnuz2fQB`i4WF8u#FZpj#>rvK&fGlz z*JS{aT?oF(iGhREycVV|aBJL9kFx)K9l!+If>JHbmX=Y8@`|(vJj=cAC!lj}u`7@= z^R0X;>QhQu5Xl7F`N$o^yZp#E*XgRq=o3hRkV%A0uG0W$V=4x~&xTL1x>!!lx`9lc zVO5RPP;_7u^7A;=2}d)A)>mt>1Tw2(9Tre^Dcfg&vH;E_RI{bl!s2`z&IULoDIOC8 z6s9OE0dU5cr49VM`#JrKInjoO<#hVDJ=>G_yoSThiThaQJY|~m{cPGaCyqC%u=>;e z{G>*d;bY32xO(3b=Hy?4-t6b5IlnDUxnQ18Sr`7jIZXxxbx4Fc80aWF!E}Y>i9J|q zEP9S$A13OHx}QHB<}tjl4s{{bAtQr5t=pTlp);BO0$az-n{;sSQO34TIJL%_?M3pcGAyt-l>zNe?mCghG^hF?;@xxq znFJIQlkT+id~uL4KFC^P)|j%TNZ%{+vX~QejV1FVQ&Xc3sM)$iVUL?m2(uljFyNVV zR~!gH4vko)M6KC%x*6QAfGduI0t&C`V!hib#Am9R$P6S7js(3*1;IvpzPkXUrd*w! zB}EtZlwn4dj~odbDN7Z;#{JnRz21+T;mENcd%eevdsxIu5o z-}J1HUQmtRg#N|*AU%b(VuwSH^M<5Fn<9gLolMwuv(GA->QcVgKPqb4!gF2=LkOwg zzQ&cje{pNE1K!5uKNOq-G7hxXXwjI%iIi5jxd=( z4h0evM4OrZd96m3u;YsV14@{q9z@9C)au>D&$ISjEy&@Zg_z9;}Xh?DVG@l5@5kD*D zb0y+IV|Hb}noia;EHfhs=8CEfS+376tgNigGO1OcpEqR&5{Z0O7Azrtld`5{!Uv0{ z2}6+gA7sKgaxZ_dj-{-re6gx@JD)1dnCqdHVS%87cpw$9^ zgZ)F}Rx21ID{@ zKJb5F=Azw+;%GChyQX$o`WA(a;5g$xHqbhp7O#dCFk#H^qnz}xHkmwvo>DZ71wk|c z=L7Fbx)>c2#Fj%|pi3-RuQX?u=SvB~Vh~Yoz%GX4cs)7fQkR#OSm|A^&dq6?5$RJJ zDaJ&hNIN!}Eb9(-Wx$P4c;^o4C0W=f5!f-{=;+AE&{Y}m6B)3)W56{|!`}?}K8*}x z#x!6knLmaBaUqd;(}1&J)gE6}cTfiOec&UzQR9Pg{fwQtB{R)gS67dR)*W0#tbtoG zuRD0Faa;zx2;~k*&Kien+caQzuG{^^fJ~*MP9ho%g}&ZDdOgr8LSEvt=u6X zWs7acgs;G>%0;C8py0Z-7mXG(v_8-SQL9MI>!k#&G&&6+t_*qdf7Bd|wL=B42N)zs z$3czBnwWp%RA-xvos-UxUrBa`$$rKvONhJSX$T~Y*5qnS3yZZ}s!~I^A!lKp%yvKh zIYs1GsRUV$?X$#vh*Dtc0mrFc@7dQMzW?EuFJJ4q?*;cCglupPTmHz{05#`Ma9mS@ z4`I>%^4q&FUhBD@AqqpvjydmX-tEZmdJYA z9{B)50P&L<8q_U-Swo6_PiD0W35P<9-g6|aq2GA zu=bKL2u_87i1a*IuMmJ(rrfHVo6EtNsXrlHFcc`+0HzW{o(TPxa-P>jtFvuVz0wq^~F{$kGSnfy+8REB;GbH4iSYtx*Fw{cl@I~lb|ZedQKth{N? zKbz)!grs&j#r3v*`#I+Hekcu2va|%f7Kb`wDlDJO3&uvU_bTQ*$`(bs4yHMeq{?4o z&M(sR(`ja3Bp-hH=$jAEHq+#bjK7wSrA^3&o|Z@tST7pXObnyv^x&gO#^kuJ^dFg* z&|Gjf8E{hvvPw|QlbIlF2WG7RX2Av|aYE$MKkCs`rx>{?-d$uW(isL4)Z>p%8(yp5 zzspAzLR&6}z!ic78gwUffh&d+q)h_57-v{SEM84a2p{2)KtQjLr7y4)w2(Ch>$qHi zer2RqB#F(NZRSbBVrnX+&?&^8bL0bUZ6vG1!KK|=Y%DKwJ=1j{&!4Yn&a`JdW?oOz zo}!qzcI^4f_0;V&?YXV%c|D_aN9@FcwFqVW7kg^pC~Ml2=81Vdp{!;9!}X+%5!3bj z_A~5h`v9sDpGBkaurVmn<#L}ct7$8JD3r^iRGA9k#!nJ0F6YMOqAhzi-y36PT33O+DmXZbt zniB{(y?pxkoU%TUPF4*f3`WBlzBSr#X-KHH2PyC}>*6YT{-GIOJj04$HY;0c%3AP! z-)Vg0;R(*T?Z&Ub@IF7Xa^&jqpb8T(AZWG8tzU5)o7rshG1qd5*IZ$gX!?PA4EhKJ zs6g`@>iefh$v)2Y(-yjtpm}%xIe+#ph|sp~bs6n-g3j6bhG)2x(7D`l5?%5Q@dBGv0fvd)NwKBi8pcNNDT++*^uAM)# z3IZ){|BA2x=m#PPR7rhmFm_QnU zqiCqoGe-S%$G8he7I%zG!AVKAcK+-#0u~IT{>`{dCAVggIg)WR8@jnGGH(1AyB$L)WN*rXLa$xVdzQGwJZG zY20H3nYZ~hFRmt8gST^YX{N007#GCWH15anaI~&j+pVa%;yOm&$K}Zh%IzKFp2(Bp z7fav$2ji-IO`692?$HURNy3mQLae10Knj9Ocw|})h6j8ie*`(`0&A%7AKVt|PkJ** zl|fa8#Y}^Qf+Pq43Na2*J~mpD=txHexQ#B7(<6W#EJ)azf%38NKWqkiPkli`#25(L z9p1h?Yf+-GqH(;z09r8;-N2jjeWYC^63Gr&7Hu4iE-5l`CNx7pmPgD5V0Qr_P4N6$ zpBL>9*XXNKF*7;F;3OTSVtrwGWp!ntEGi%uwlTX*QED{Z6j%@#;}jqEhxrPv6zhi{ zkvl!979Qf4)wbn9^%#J4-YC|+sg;~F1V`^XXpJP&<;+eNd|3NRj`0Q0w&+8pu5))! zt0kS%-Bpi+`T)LPwD{dN_w+dPpSdr&yPt_G9LX@Y>9|8lb&Vfz%xN8;u-`GSE@^_w zMS5#+4{|2n@VH_Z*U&n3oU3~X@?+SQAnrOo=ISiaRK3hwq{fKU?)|oRZTo5vYWc+I z7<_U9Y#C2QhK1MYhal!hg~Se72p=MyU4!qW%1v_VYoOPBcd@y2=ok+Tx&2-TCluNu*p+(iO8eN04zq$-4l2q=emO(==vtkPHs zS409fwuGUVX(btocT^kUsNH}(+;u3O{FQqNuS+ce%^<_D0ONvzYOdu2B%(tv!Al%1 zV!gBrQ5Y^Vq~#F)Obx&&HiI)fUVC7YdoEV~XBzFBC-8K|@y z%@*_kWaS!*tB2OsSG7$8(P@@{3zvaG9)FTXPlPKzt^ zg7CTCCw~Xt#ky~Y7_*4+-g)P)ryESd{mr_$8(8;@Xdj%L>F)_Dl=6J!RIL2d)n#V` z&2#tXc>+)@Yg)Gyk8|T)$64|IlXa;PadTJ~5Dr%IPSnM^)CX?Uy6b{hf3hy2ZVBsN z^#k-2E?7_nlt;OAybrB(xds4ot1Vew8*txoADV}I;OSuh4 z7$f?^ljEp~H$kiwwTjIt#AP^|f;`j~k0L8XhgnX-(s4|Crhd9J^pB$Hwl??gL`>fY)dFKM#hx{ zvq+fKwZ~-`2~b0AC;4rMxgRrO!nf;rM~gg(=z+(0KWRQi@+ZH3{-1iWYZltFpz>DB z_^8D)WgQ{Pu2uMOjxj)>|7RlNYhOl}|b zEh>j#mAFShOMEl64C#?}?eIH(5@^r^f~+8=Nb(H9k+6g)7|hb~Nrwxklg{B8*%8n* zY7U9}%o=n4Zl{Sz7*HGog$6}vh?&|eKU?=|y@GWu=xcjsLdou>FaPp4>!xMhZ^5%# zbIZ%~b6h~zv%|1NYuMCvr-OW0`7G9D2_;Sh>poV4?N+#s@6=`&VKXaKlK{dQMV#F!52fUw#mBt2dD7JsF~3IAzR{#;qv-tuJT=Y zOuVB;>eW7xsbQV_954$Q5EJDsk!121F$4jL)GK@ejx|H`0H{`~Jy2Ysu0c&Thu8Yc zeTRSoWoa5c^a0Y)MgwkSor0SA0y>BH$s9b@Io%^70QM!JqDNpvSLvftqfzhL#;3GY z8U;oi7HA66nl39_NTi8eK%Jy1;nG%nw%ciSjYa=bR}KDpol$9Az6@Kb{LQenvaT(; zIQ!Fnxwe+OM&6c9-qN+DTTtsPE_G+kYr8I2uUCDiRYD_0*Y=`-t) z4U!zHXCN$;ACRAe&y3y&1V+5i6uViOKjnrXYpxY{l*TkH@Xy3xqY@yw1z6EuB+lJy zJqJE{CVo;H+k3)vf?zYt_ee?XDNEArHT=&~EKIjcvsYHZ25rm&zclk0Cn0e?z4oDOnTY0<$5}eEj>>_ zpkOhyWXsz)D2iWjJ;>(XAj!A9ZAUbCy2bMCo6HlW%ChBy`3wTg*mt_Cr2%>*plJ z(p}S*d4xt_q{EYxEh$bAJkioiL_Tr2Y(D>xLK*!QiWKT2J|g!N;WJ3wxLkmECXOaF zoh9WRU1!P{`l;HIgjY{;msm%o);Sw6v`!v1YO)0?Js=^Yx~smc)pwS?p?nRBPUz-i za3bprLd4?-hetJ;9}!YXAf=R`Q;*JvQWI_!#@U%x1xU1($uYf07XO(iTwGNX-ec-GN~O?uBDPYrsSOl zJ#9>Bmnn_3k0zyL;{r1#7V)G_Z^X?Ugc&$_jb>%!deCKUg`Wd7y(EcQmR`8s@$RKN zm@@t%6N*ilq#0VSv0lgVS%I8asGP{D8&95k_Ji!&{-g$6v6kv0+Dx%4_c(85?DFv{LaX4puv2&YD>1sY+l>f&rsv~rqg=rHjqypLp#p-}uRI%%vT znrU+Cgf$}?nur2_!I@$LMMKq6(b4(hd_ZU{V$AO=)mfSy_60q5Hq< zxqe_wa&CnlnEmBZcDB_VZE_<1)%J9w4ws8`*Y+NifTU%Me|iV`@EB&luLhgO*k*}p z-sfi{Y{ZL4R<{33&whB?NsZ}wU`kc{8z!}vl1VEl$U{)?q2UTA#ru5GRC<`1xkSeF z;~FTBi8Z>voY!9N!{CpRD`N)^?mMt=&+Y>dqk{47)qB6{m9`peE+@8Ox~QOa*tv{O zaOL3l5QD=Q3QUTn{jr6CX*hO{<^nH zrU6lyOa@~^ZvkK$rl?i(`}cAne9^ABW~w_|Ybl8H4Lb0g$7ji~}474C?}`=JKS$SSF* zKwg>8`cU7r1jUk9m4sY@F#+vj8{A^|pBIqy&uz$<*QSOb9y6*uXbJG9=dXPCt6uTg zY>sHdPeTP01yK?l)NEng(4zr_gcuV&4S=XNIzb9UfbdZUm7H@T$JKU3q5 zv&is~ZG!n#yIVp|3V%k?P2i*xgp6$b0t^a1rOe6EqoGd!m+h<5gdVOaIW@ix#L+w& zBvqmdx_o?+JgFuENDyCnf_jssTW{Fz5ogWgt zG^myQXJqi;-a#uqDcLm*dZe8p zDI|yH?WE|XfXp+2OS!|;YYyG6@#3{3RE|f2@?~>XcAEcdPP$C$y@zrC8*VeJwLolh5 z6r21{*P%eN*aEhliqPlBS-ze>!k1hUrk&=WobTYlk~m%aJq|YlB(4Nmt@pGRe*vt`NxM5fROE3Cu zX#W9LTaQn%D}y<>l~<-Mqq5~Abi<%Xt?H)TA}VMNMT9ygMWJ5Ce6l48BOuOA ze%Fg7Ef>6oEx(HlvXcf|+DTb)mucB@_JXvX)zl$t1h&mr8ax|oJZk8FYN8B(59Y@r z74T>yd5UKw-Ib&f6$Zb8p>OgqEJO)UH&xM`7fDaX9c1lXIUSqHRVge8dE6{sK5^b) zp)t5GK5u#&#;&KY(_|xH6XFe9g1Ebk(P>zMeu4I`(05_dUTO|}=t9BBFPp>)4F+;X zsY#iCV%G+ED19-EX#*S6o^0Gd#gQOja#^Dg(#>Rp%1oRk*!B8c*XVdyA_;K{!wuVFn?u3uIDGjwi9Z9g*q%P zhikdWLff2aO-eM5ye}hbo|PmV_~!Dzx8^i4DVJW+&Adg$%!n7A3zY7XZ1n!wnhDaG zpKc~yjrdhrGkkDhKO4JQ$;g``49S{T&XZMeC85N?a3yH6w-3&F0xBJ=sfAd90%?zi ziY0YF*nohhFf}*@BR3U=a8f1#s@z7wLVhEd$F6?XzNRWAfc;WS;4QIG?SJT=!G3tp zkRF&$lQ@2$uPT5+i?OsN0(qvlERs2q8jWT^DqcyvNN>OdS-Hd4Uca3Ck9wS@$D55sMYHrX8*?>ltsrnzX&KLaua zc`pvjPhn3s!370R;^9$b%4cH8yl3v9c69F`9~Mjp|GH#2F%`N})oBNMRTO{haj zGl|(0bzIb~PjXlD`34H8noU*sqaP1SpXatklK_4ow($ATgrZs{>@jW*4uOJ1ok>N< z!~|4I00{C=CWs_p3!MTPIE@xUAcMI8@4?pxdzzXygk~OE2jD_fY-C?xN_+)G_=J&h zzzifID%?;%gFZP=L#aXJWta_BSzTG7&5Gj|WTlC_khm}|?#CQa2Y{l(1^USTB!@SI z&51KeoG`dSa_qwv_i#LM5*_Cq{WN;x^eVY!NJP=AD>GJ&eZgXk2yP(((HD9Y3NOSBTqe^m zw8uP#we@nKwatN40yg=hrcgTD_QSUyFJDkK=sQu+&n zlYSW1rO;$^t&z5i6lLTwu24E2gENN@rS}ap0&{Q*sHW&PAfyG=3P~!#lmNM$0wxCO z56HO0beQLmhB>2Z^h=Fdz?_ULPX?{ad6$=)lyPmAaSH&e>s5_Am;{)0-2?VXWz7_6 z1E>Uyk9=9k-}4LIcB6v(bzKn?K98B{!Z4`Ik?`CDOhZbkYZ~{A7QEpeQ_JSfMF#<| zXvP(C+vra>SJOS_&Akmu7)4IXl+nXY#CMEKSE^j??%dqP`Po_X<{s5bF~58`vr*I) zGOp!aBkuwp*O<1EuS<;wcgdT<1*+l2zFLgSGE4FW+#nH^gtI?abP!Q>AASmTwE(B!ne%jCfRaL$Fulw zgmoxMNaV@AX)@u|VHop1)q??{z$;7M%P5|ak|UctnQ>Pp7lDnIDRETH%BR$EU~bb_MZlhnl@lr?ZHbfecs3r66|3iYFz3Bx zIvvOVEAzy{Q%)~}=JhAJ1vy}*+!;^$63mEKtgy>cLdaR62(smq@XL5Y3zJ(iE3HHCVVU(X zAWIw@IC!L>Y1Rl!F)h4P*L}0<1atKxCoNhIRBqF(&o=7q=E{3~$u;PUCDW|$Qc~y3 znDrq@WhB<5&F;Te2SU)aP+^YP8~#KBJ-mJV5E)6aEa-T}9BPtTX|3_8cxq5nwtnR@ z%QEW%LJsxzMboT@SR;KFv))y_KC{GWF|rPhKnbWYdk(~!6UL9ne$p0zGv))L(rm7g z4Ty@#{+QX>?qauNv>nYNcfp99BPSD-w~AAs8c<|#l>Das)W zaOA3DRq0W2Uqx%pd>@9VT-r<(@X&E$sujuNsP?K{oJ3=7miY_(~tkvc(*l z&}K-1OGvSU?3wQ#O65#YQ48#>IYleM#ZWzV}U@7Elm7aydva5DmZMcE1T*=CYk-)zFAUiHVU#uo~_u7$8pFwGdeQ7 zjBF{d6~@quH4s_G3H#`sDq3J`j-V87O^S<|X_^vOYQjJ?6|TY#5GAFgFVN$_!pwCd z)MkjS+X2Hc?01w&&Y4TMKDv}a>|>hJ#1TVjmG|d*)rL?oPs;3iNK%>B*s$~IicU{| z)|!_iezsAj#eR{m_-J(nh77^bc~Opo{FrZoce6wQ4ZgTQvmX2i<-dFxR5$YKT9fbw zFuHL5+fRu#B5}L!))WkX>(Iql{E8bmvMxY~ z@A;iCzWD6(ukL<(7X&4|T+L;^>fJpv$Ce6=+TqD9i$?LN+@MOFB!ocn4AO9Rgwv;D z5lj)lg3!(_*5%2Ahy$oZgE()}91%W{>kW7qfuWuUw=*K7YT&d|Tw!>-KayjMaBykfHiX>#a0 z783-Lmv7&?Qizk(@?IQLX?e*@|FKIOe!;XcemI>pZM>#B%*fcXJ4W8BEi5iUxp&># zSf6W}Mt%?9Qc?o;J-wmKAyQz+i;J8xXwP6UmD-0g^7Rh#-jv5H7w>(J;^}!A`I`v(4JvT=y~t_;`J7w%Mc}bzUmq013UW?`cV;RFWRzQpBighW~^FS%T&)h9VOa*vwB?6hmWxsT8AR zSB++r6NKXu(goB=G~=--Kalgu#>}|3bEy@=5GV?G{3b2_h`dImk7p5sh=_(I`Z&MS zW0(eZGOJHdmo5UMAbvS&sAF0E{lWF~S5Rf(GU}J$m~BtjYDV6v1aD{lc(d7QndFv_ zR~Kg6vlJrEcTr@L{=D-(4gdHf+M+K$`{3qBw?F>)*6ke7^fm$b$2(V!Zk@Y%_b=PM z+pvF{1ZC%scaLpe`^&F-GnZc9J7m|u41StjBJ6X#h|L|Z9zA@_-(B1DIMrn|)cjx7 z%Wckmr#q|T5xUIu@!ei;`}?o1oj-Z{1}F7{MlHeNHZ|}pkT7QXKj6{2z13@ zT$DLgv%gR6s7jOTkX@@Te`AiZS*!=x7L>DIvv^f zJ!SWhBb*OGbyKE&zxep0Tc6yz^~opY#{4bxnaUu@k5|7oazGvF^heuCL`>r(Foej8c{yUv!mo#8uX#WS(qYgR zKzqLjDj3WdDTIS=AOlbLwTeUz<(8T_Xq?V)l_^?Pq^MJi;R)v{329~ek`!73y*M2} z(hT&HSJegV)8a`Xl7yd^=tBXblTiq znm%vdUlk>k2r&flQRblvr_T^U5eVpo9+hwhKs6fP)0{N>&n6~RsQP_)uf=1?U{NNnN@lCf%I;E1RZ6+m?yhgF@OpBhla*V% zT4u|s*rn!TT# zr(Q7bm&j?FINg_~9S$Q)XGEyx1hNb>sH|ZrcAPl~`e;EVEX;(s^pU}Hje?!E1wfSQ zj&f_|q2a8e;jdn^ax>~@bL}G&oh3qtY6Mljqyn^tVCRzdhhxaeB4=h(o%F2*TAaiV zCPpnF9`xXxufM)~VD;s1dKO%@>tPagsl3?-B4r^;(4VdCP}Lw}s4#N9S6gnIO?c;7 zH$P{X)4{Z0GWieCF%afB{p|01uFsj9k8W%;p#B_t%$)w;|F3`i|K1vQMd@!yFsEAX ze8yM3xQIoga5Eeyg^r++R;n&?#~t?<@|ioo>wWU8Oc?ViJxCs zXg3$Wl-aIy=2|I!lI;w8n{v*4u3FnM+fqJ%_jkSU^HbGQU8Q~+Brsh}u&5^%qyk+URt?r zX1wCu+3iykV-7UEWV8JThAo@286F8&%2<5OY|WucVr(|sonL%MHv6&(dHLusHaq)&{^LLI9AL40e{uUlJ~qC{k?3c^L6 zKcA-dpGm+;D)lvJ&#B8oVau*bz@3TF5pFm@fk?PSHpbFOP}zqT;L7W<#2(J3*)6EN zo6p5!d0etcX!u8ZF`Od0ZOrtt8iGK=3Y$1EA7wLgwam;zV8TX*AFK$HC^ox?UMG`a z=*8kO?52C%w`b4(@aJsKdX&W?-yM8w|AhA=LJlQKV|H$?LjftHJy|5`&m7W4YtC_) zRiT`R+vD}fXnNFt=((6~@OTcr{Lel2Cn>i4xUX$9O8)ZX>Win znkVPD$4hsh4xd7&ITJ@H$Bz;t-Yk@;H-#-YUY@MhzWH76#zn~BIeLPXBrgqZD5P3& zap7r)%}ehhj_5Uu&jsf)+L1*v1xQthDtDlqV-i$E0}XbYlH*V_)p|gt9Z7QY#_4o3 zEg{+qwIa9yhn!&+kA0GTDt1w%ciDE?`Btzl02l!|n*dCz%KqYM{-ap9qHXhGl!KoI?W#hvICNvi?xQF89U`3{3h=)17F~EiNrBfSrl1 z@^l^aJGaG`Tx&1PHXGpOP6inn_``G$Rbyc~R)~c{8?P|kr(_Fm|8>)JXLn5Z#_oMc z{IXXssapeH)A<_Hohi4w}Yd1c``tKBf3EOfX8&(EK_zit7ue0Z8NeY zF}88IpbfMT32*R}cmkrEQEh4zfux@l2eN=nM_Wbr4&FDbAfl9y(k{rLaVf}6k}SCm(I2%KGZlQl9)j zdHKLWmTT|Z&xIe|J1{XZGG(`Wm%pHf@c#JZvvnZfTyBv;nQ!n?d`8H7nJU51{l#o! z^V-*sP?25Oe*BsIfG8lvVnXe8U0DJcwgFUErzD=Hyd`mfr5u>W{7&8ya@bUjD>Y3+uvsF>6S!pG6qhV+l>7AMF6!Ki6FOikr0A3mUD%x^ynSn9u z1#Ucby{Z$y0jaeR?vP2MEMazkl3ncf7k})YL&4b(l?778&*F8c=Huf$Ox^&AFbmMW0b4?OJQDJ3mV=!+BBT8UhMJ4)!vVBICaCk6} z82r;tI)sy$Ybuv{N|&m$Y+oe0oXs^sUwIFZyK*2%1J*(&s)a1fwz za<&{9O|-}xP{#MCjsmx3$awjJuj1!e-x$+sHF-$gGE1nb+u4^C{`Ux!A={ zBF)>ztn>2=v!tiCi@?FM>O%^r6jnuh6i#~Xy6!4g{p2UB=Fj|eS1U#HuI}Bpm(58B z1_wu{h9_BT!+3XK)x4`%b(4XEO4+n3c^~~M?rOHiZ*zaSAgk)GIYu;>^h`>4kWgps%(?6@TIyMMq z&BiC~D4)#E&ejWM+C5&hnz*;~>uYPPOEEsxl(*hpUYptI3Q()hpE_~k`0<7EYzH2a zabo-Q(FV<)dHPIjYq)Uq^4Us#eqsIOyVq{rzJ2TZrQ>T&Ks6$lh6^dzK`&uWNUa_(POnJA z?&7sjt26XvgqBH#2(lCr6rRlEF|Ng|klhK6do@ z@y!(oFC-ul;9=HZjOObt6iNk^QiOG zc#J3s1}=PQb7TF`asmY3=5H<@+BmYv>_xl1eC8ylV4*sX0neO0efrEXI3lIvC(a%@ zc{0Dbd8mtDYIYZ6$)L|-oh~e{zjKXK@b2bZ5u(9Kfzq+|iT6J^KR;~`)Z5DobD4>W zamI!0RQKKhpUd1L?(ydjzkTgPPQhxk*6m`Kg%c-F96zxPg$vGnt%_}GM^Bu4@BNG2 z`Sruc-@f{R@oDsv8}FSs+#=(q`qLgBewmT>!%l!Tf_KJ=8tE>!q&F?L5Htqb0V)}@ z8d$3|X$dzOP~%^Kj`#vHK%?x|_!EFz!643})5)tLk0!4sCBOzqo{%BI8?&<9iWN9q z-k->%N)t3sD)5c4szeIzFx_m%R#l-vJ~OJ(X1mjX=K(USxQ&B|uyK`?5JO`M5fljD zzyT;t*HsOX6arvqObJg9dzjVEG?yTvQA8=Zr;Cu-M!_$8>g0)I8_WD&AEipr>zYDg zHpfUvIrY!@f`9s_H>Ni(UB9@Tm>LD=tknxRHK)C_xY((+AcfE_(w6PztSvh;^6o;VPg|)^ZICbV&0}|MYlV`S09N*Z=uFcU1ry&w$-{bhW zJJnr#=lbniH{RWtE5b>DAi}6q
  • ;Z_ncjs;#B@*%TY}SoJuGlcq6YE?ROT+qeiCypIGwx}6FA@k*Gt8?<8rT@k(9UDieFWtbqlr4~TN5)X`W!i_N+}p<S;pLVmr#TuO2_iu0Rv(S!0?Vbc4NgM|4snG*$ z#hDrz9i5n%Omk?>AY~bcVLv!HJi`B5-LA=z@qXJ3nnCvID6KT1$DCoHQCI{%08?a9 zM9pd%^+YYDd^nk}wp;WbAXCod%ZbnpEm=Aa@vxs9=_|4={ghqaW?w8<4;sg=D z(4sTjo}*7jVNE_q#Z;^Imlvu~=v$pO`~k9h$-W8zMF*70;c%9UQ!W_iDd;YgxE3JP-aP1$;uTfZ*(sS8%|h~1OK^Jd5Q4~#g&GyzlU&jMQ8 zwKAT$ZA9pd@j1-Tbps^O6O9j#j*gA7%5co##rhKz88|B!;#5*X5N78P?ix0tUyw>d z3jw6zJyEA2Oie$DjydF`Zl_jE#!Jn)sxaGVJX@dv=h@1Lj zW={(BWQTRc+#DymJYU6#Gt~WCWaAt;+Gq&3;s;?a;pX_zsvJu6Fij`V8_^^bqoTDI z{ZIOSrD7yionKp6TI}Gf0o^r1EcyvqG{>M&C61Sj;-Z=Z{G>87+wD5dW|BkN@ql*} z$+hlWyWK@49$^k5f?`9X)?Y2t{pSSC(G66)t8ZNOQdAbQOqSJAv@GfrPIT#+B9*oiy#?|Cln|Jc z;QryFaA3ksQG^n9ZKO^MUgN&Np`e9_q;XVd;ElHq>>n8&_ostX1H+SEnAST-fF?4U zpo|(%M;HVg92giL8D%&2Yu>X?ZCo8585w4(9+~-xu_@+)8Hn+)Y>fE}&;V{52SDkq zwCkA5K%ko%g%{K+kf{(=>!_DJXxEF$7~Sx)1kMAYc(KYr2vB<*vdK)g(VAU5wsokI z)<$w20Mv5K128dWp_{=Rvj(cf7U8_|nPQP3^)5<8r0o0>rW$@e0CeR^$&Jhk#TzfC zGCNgF}d-v)=B&>%V{F!0@oZa<^-A&%gc4?|=WtA-nn~ zsIxebvhvR)HEO0eXUr_Bv&Cg4;DVhZ@5q#ux|*2U=eLrWiHk3!h>yS_1k*F37h+sb zdLSsJ)EWHb_2na)nt^x(!zBZ4A>bGt9iOx@f;>}bG)fG2PY&(Pq$q z%?Bt*#I`jC13QyEdeR437pH?(4sBIP4;&%VED{D2fKj0=yb6Gm#7)(^)+qq6Bv@WR zSoP8qbNLfgW)r9wmzr3TI8HN2)+!UQ_$y;@Np2RB3RLgp0i}hCI9QOn01sI>&&#B8 zr|;ko6FShfMXP{QR6*1LVjumxno%zi>69cw+>6r;$;ddegyIyE0SHSov(wR=DZ%0j@g47~Nmzy8~P>$K=a z{5v^AjA#i)MCHrVqhu(S!cKt=cHyANq^&4HLr5p)8Ndw@Ka+{ql9$Oq)CSOq30W&I zuF0_9_@tm^BTj@wtYE3AD9+8b;$%mo`~URTh@Y*4@StYru|ynz1+}{Q zIBVnZ^vFY50x7l?a~J_+#7zZugAl5R=%A%3$sUH6fJ) z@EYU*avDY-NeHQYIz$8h307XS(`mKmTFsW)CR#I3Wm=iz6*#Cp@GhAk!I^ZPULv(- zp~OgbvF|wI2xi&9vzv`iFgm7P)Ysw&OiVE{BmgQ*YaYCxJunbH2)x`pt~ydQc#hUI z^ZtkifItd20-;m!g3bnxlF@CFC6k@ak2UNolLz1U{pvXfz+*v? z5ckxlhq*{xN9m|uPT;jo)-1_$24veqWj8*B`<^K<9L`U6P7LhbGwNsHmi9$=iEl|G zgn}FYnjX8nB2jV57lYWuEyO8N3E`KRI|JL`C{R%Gxq&DVViHWFG#;s!Jg-}+0sLRS zhQQ+wF~{#?FIS&-X*+Ov0HY-sH77*(Y!OY8Y!OhaDF;y&o(&X>MIp4dSh z>jw>0jmxtVz3dJ+`;kJPlP$$ozYj1IXf{sOYyw;aL1NvGk233Ny$wni8_Gg<&Pp(p z!t0PVCh{!ON(b%E>8QstHF02Ia3TQZ5-qNlQBT9xQfDd>bdm8K9I|SP4>}&&8HTOt za89ASH$FZ@40K5gmsrl+C*=XYf`(+0fk3l!R>?hv z!Dj&sqT0fraK^#o;{t$i2Vtb8efSH~0Pa51DzRH!6$mLn`f30Mg3w2)Y~dZ@QO1W4 z4E$;Ln{T}Z(E7*S14APdqk|)pF3ZTk-rd6vvQoP8los&*6yen$AY1VVXEgf;GC`gY z^YSH7ituQt3~bW1XDNn!FAD9b|LSrTS;QoySxT5R@>$p^?EqpEjX%u?GRKUQ_ZW_s z{!)os4>rbK^^xqd7>#C95Y0qfeW^%f!3|?;op}`LDNz_+APREs1*hr#kpnZ-MWrMH zBnKXuFy4eJIB?rcxjnyBn(_K$DG>+8#wT5g*8DOYi3MZ!6ahdm;r~z6eXvJ%9$A8} z(uKPB5AN*jJUt^%Q{JH9z4t&O;l20eSdLY+@@jcQIf_)vyOz{aixQg>iQM;Ga%LZi zWEmildB6A~PMkOqF*7mL*?gnEu?Hqe&XuMHvv7<(=d-$6QKKVcx($^E%zji_iMkyr zq{K<-us>Rgv-KKE+3FUzcze!$Y4eyXJfWLLBtVRt>w2_F1#R)${m|Z-mpG>Yd8b&0 zZo6R=61XySEl=6oZ8mT&vcg_NL*w;^=8nOv8WD#zVl37zOvU%f4Y@>$Oow0K{HEt| zfJ_i8G-Wh_Q=6z8{pv_%GWArISABLJZB0#AFFt$j+4EPfpS#f5($?MC($-6H)?9zB zb1dUV)qjjmiinJ7$5Mn?iUY{mEEF@JPSsNbmn^x|h|e$B^k)c4=HrDzmpJY1I79)C zCRt!wTe6)qJx|Gaxo@PmWufU5Lz4}ztWe(HVGX3De2OtqPCpL7w{NTqDtx2c&yY|e z^Gf@WO>!u3!F6-z7Cblnd^Xl*!ou*Wj(lVvNq|U5(|mCA?$$iLz8(=(wcft5rQ^G1 z$QZz9DvDEKmW$Q#u;Z-*MqjCn8qlJ%56&9tCxioeGy>8^jd>?Vu0R5M1K>MEu{uz4 z0I!7Y$Y8*gl^t^-L)%oPA!bD#9ULKu(ChBLPJw(IIse&%vwLSw8q5eb;WWA+>TGB` zHMrra=}`lAKyKq{`#YPiT)c4p+^3!@KGpN|rQ+gOFV{EqOfIBp3cS!o;XP$V1MAm| zp+c2DH#^$1I8&do%yN(;sQ5(|bXc-uN?Ko6yY=kWf_}L9rG~4QFJ8Q1O*Mnqsr3A4V%eRm=glLqDL1Hn>^Li_*# zS&mO=b`Flm%^b#HK|mo^Rk9+1756M0=R#r?_R?!rMcEPlmX1)6;IMrnb4TZUpG9~0+3DAwU znF4;4Ep%RN$(47l)7CW-8(>V;`K{yI4<8>cMwl6&LQV%E*bQ{LwHzauGyDklv{u!w zZ;!?2ld~oGhYbO7RJ(%y>@^XSf*-V;4)xl2>)BR}fV3wHb7O!rCjnr!HQcDbiVH&^U&a)UvqUMdPt!hs!H7(^4meinO#w z`%qa$C}wF3zZ*u zfwGd6E%P#Ac_InVh&C{nsox90myDpbt+}q@Q6n3>exqI4qO+~J;qrxxEp%>>pu=?_ zixpEdOk>Q*L5=texF(h@<`%O9aIL>8R!--EPYQPZ7-?8oRajn8K@(j;>tr~y+DCCT zq(f1iQ(9+2f-;P_u`n}LT{DFI;r@a&muFyGB0)q5n;Q%CG9HELpHf}^fjfz%Ez`)s zm&Oms&qs#ebbU0cuPR-5!?w?$*vE!?B6-1Hh+7$HA0v>g%o|wQeB(w#17ftZZ(?=@ z!GCaUHt1#sxXXSQXGVHVk-(w8A7J%#|1g3a=Ahp@`;9~EDyFZaH}mr|9-8YVR&&nK z4|*Yej=^atfn6rR8C#Wzy8-#7n(?ZyZg;Mw1-aq zE051$T9RjaNCk>5>4%vacoKbf(hY#jCzK+11ZjUp>S~W38S0j7clC6)_B36-APjuu zBme6opZMgtXFv9dXD(eo$L-AaGXOw7nsy|;{gAFX!f`y##$lAbLG#7TeSabNOm-Qx zr0ctCRe}LMrnOk$ZF7`c5{*R)2Vsvfb*gXM#nuUxJqn)7W3fcO@Z|2nod*xr43Jq^ z-Z{KuR6X8Z!nG#lsD3`Va^?wATEnQR!NPt?H{gC6#c;=X6L;5qO9gU~&$JHNEAzn?#i_**O9PXmo%oU0~{DqBt!}H_I0qs=&W5(Dmo7g0 z^rtRdY3TtV#;Sa?_ovXMPdte@emKQ9qL(`V)YH#Ad+}2JHF;uV>oqrJ5^hP&%bms3PdR9UlLU$PKdX9Ld_o!z zGjVBdkn+n&lqD+;9Xlq>B@VmI1e})E1TCjBe{T~gmN0~x*AAyaPGKK}+1x!kdGPr0 zR%S%ms`uc@Nw_=G;?0cV3&hH>i}^vQGoQC^VLYgWPN&s`*B1`}O9F z=kYjCU%b{a06R>~X~o5E6Ce2xIA!1+aUpq=mO@+g9CFnN!lI%GQ+8F`pE*g^LGC#; zY9aH~L^0UiU-aJU7-+A*dgc5xAOFP1Kk?L4mo7ei{#s*OeN$^sMVS>;Q*m9arVRas=Ol8fJKOVes6hkNmlh}Gep|bE;kjuu(^H{#y;8IkO|7=w2<4)R zle)URQzD@iRsdb!IXJoh`1x&TpsDZn!zU*?nz-0qTFsaXZH)WD#dB*YQ% z$&p2)u++48s$vr}*I|3>kiRXDgn-SID@r#XTLZ4aRUbbd1ig>?8kn1t)O? zv~sg?E`Ve)J;gByGK>SpSzOtY*MDtycRkZ5atev>r)`7*%)J{lJ0?>ig>W#P zcMg_E7kK`j5u^)W!CC#i9Ubz5*7ly!(E);{hWaa)p83?rKk%4WeKFdanWnuBnS1B)JUF^&Xi%|MRy z?Z_+KGB!bf&XS6?Kj&|gG(ed|;@kT>e!;kwwfmAaGLH|>9Ml(x_cImVP{w zwa%vAIE`Z_CIa9`^d4j;D^=A8_VS3_S=sc%t|$YrD}TJZyq6AHZBl!;+=Z+{zL zhYdIOj_%&t*)oRZ@YZwBp%RWyEJqbb&u*SQI&*OS+V!i~)$Dr)4LhQS=*xegz4oT7 zBLBrJz`K%tM_V0m*l_jPr=8KYE0?d;yR-Gl5C(M|h@-CMJf0w}I%EQfx42j?Sf9V? zMyYe#b=&+>QkS3=94_Z`O8->UV8T2wf3%l z3jopOTSu96!W}v^CzlRleQQxu0zElUQkx}n#$`FyBL0*Il?Td|LwQCgD|qwuv@T`d z>YFnU~*ey`S)nf1EkPPd%R&R-NB@O6`Xjs|C z6henPM>p@?)h@8J$pr3D9jZ-$6YDX7!!wh1IV;Pz^_Wtxph`Mg7*E)t-h(d4-sm{@Z$RHG#*$~OwZ%o(iFycc({_ftdfe9 zN?l#X+c%=BV_sf=b7S506R**vvjT`W628~qtCsrfjg8kXoqOh)a~FZbiRf&I z(D9XG&H;I&zMJu9pK*ZR+tPeLhsWL$-TI~;^I+GXCQ>@Ey2savDHt*vKp8Ym~aEc(A83XG&Gp8*bp zMStH)x+FXDAdOfmP%|rbV)u$Yx>#e=5s}o2@on5{B?6Yvl)0bKX$0iq?4$3z> zZ#ch$n^{h|ZCHXx79 zU{|W3?>@oj{pYvith}+kZO3mn4fOHB@dMd5tfk>zXb@C0I}I?79}~&Jn*ih(foJBGXoo}*zZ38EMwcP8 z{GDy&C`)LOO86#w5l3f1;t#M-Z?&r7^~RHYNPe)Sod|U)R`_XSzIai>Fuqp z@1GoO`n5($oP)!>0!hcIMp;ONX=hw5)reLE3PZ@-u<6XS)oVHWXcWmt1)&rrB$Y8V71;r&G29pG2X)CtfZ;@MyU;X9q}j7iyQ?_R zRdhbr(b?Gxx()TK({%OFsp`0?zai;hAFWMI9YW5~^lXm6_i1vO;#U4OYBv~D9h2j- zr-I(NqMi)ur+&2Q2C_wC7Bh%2kVNMxkLrKXjo~lmXO>_oV69l6H4t+*^XOeM38iAX zRBEGc^hJ3T z=gs%GEnz_u-6A$%<;G0}S&+-6j@%8?*%uza7G z+YJ%CZLmR1z)YExPqWHwE}M9Ky?QYOAW`*Qa(J$a6E26?pbZRK8{n5%2ZPIxRzS)i zW1_S{-k`m;rKP>YzuTB+i?Gu@RGIaTAWWGunNtDrc}!WSl>aSm`K1UNl1vqRbonFFD_k#2fJ;AT+nP6q0rXZoii`=^qLXS z)Me$C$B(rwe?gDMpR#+HC6a-3)KW@|4ZbRuw?l5wS#V>VH&}2Z zoqqSo%pycbE0c`J#FF*>bfZN&6P>8_E_T5aAt(_@>C{eQ1DMt6g%2;klnO~KKxzm4 zmyZxS*3?X3u_g`x)JG*g@NADF%R&I1Vx_6|474|NWPhU>_d`?;V=RIlEXVjbKWCPn}F-6)MV}ZlBPK?VztiRnIH7Bk)|8hE5X`AuvZF!pFvziAK@&<$lPXO&G)Yy>?wU&2{z1 z4b9!W#QO}Mf*!5zY-QfQtkhK_RU*V07SkD7Z^6TSkxngkOK=gcoydqLQE|DoxuLbk za2XY3Km)ylmyyGn1hKTN2t>Aw3>E!hK%|6@=7lFom2wfoP6R2!bSbw+*O@g2k%z=M zV1}}&f)kQn2-ik+Zx15SlGYaU-z*!$e~$KAT2pvM8EmZL3Csl{x8^Qq7OC}? zk32MMK z{k<@zUTsrh#<23~GyArN<0qT0p1;u0DiD}Tkq#rv*XSKYh|czQ5WooZ!PDU(^&Ltm zuN|jl=z**ubEBwb%$rw}uwf+nBtyx2v2yw;L=&o5fODgZu|i>$ zydpH4=nky#7W#~Yqa>tqW}9fEv?X4!?(m*UXsIcsTk7d-wHdyYH^Ed<`hME{(Y^tEfKmX#{~THN%rf~0EvR?pM~HrfL6>V=Coeua z=5=UAkOIfI?mZ9&WCJVzM6j>l#Zl=NBiF|4N#i21M^(9H^ji}>Q|9TFv%l5{`+K@L zfQwI`t8bx7;v+m>!jT-mO~ysr!)|@KC>ybw1R|m@J5Gpwg4}$0R-%^2k5Slcza z1(l>g4q#x_$c2z99lfKt;goyB35Q|gSlrpwb#o}$){bOc(*h45`A^jdAq7r(Izi2T z>#I{?-18;gMqDS{tb-#J6@)Xe6+>BBtSs0Xw5KyqMv=-Ap8hKn-Zw-h(c*PB2s!;k zO>+`AL%0-(poR6@&)wQVF5t)QIn8^JENyM4{9G9spj<_e&)KD2O#F^)&NoQiOq#Z4 za%_+bu;(Z>%A3ZT%K+08a(E=~H@}x_%I8?yJbCiyW~xcv@;++s?sF1Xi4FTg7=BGU^Px5uCutZ1{)$8JhP+^29fV~q0ZnSb|;M@Fm%;H(LT(np31ZtVipgyhb) zHgK%NacJl2o1DoDGN(-SEj8N_f0msZ8Pskm3jW!Qb+S+b8#i0giYKCH>F;i9sBf&R zpJ{2XzjURgKUszrk^X+tLcIaclYd4H#IsI}sk8b<<|{iw?wd1GW+Yj6fsWu$divTe zmE&z^ri+~Yk{}t%2Wr8}Dht=G%+T0-TA}rEk|hMM8ZX)F-WM}NK09duJURCQr^z*o z%PAW8+Kdl2aui;1u2KTwmw9Z`e?IB7r@Mngb*gS3#Oj*DAMkQ;0x5d_jqZD_r5qq(WMxv9Oi;mWm+kyMVdjqZEn z(!@YyXF{#%qHAoiOI!R5ST|6?M6cY*7R+hq=lAd2q9aH0O0l+2 zUVP=rzU2v{3^^@AaO`SfrCfuB-#yT?zXH86K>T%eH zU08Xt&Iz>X5t;x9x?#AsE=7X4{GnNu$T3>TLklycUc`isCC27M`@tyrm4&5>5oi;$ z8_T5y6qWmgr+uKEv=oup+)Qn7qp^3~#R-X|xJ4=yu!OpO`m9=7fQj@LyoMjJR08-uTuqh=T4{1=b z^pYsVUzXwmjg>Kkh%0zfKtP%o#(5`Gi8btmeBRwXJUODY*<8q$q++hmA#1Tu>E5IE zW0gFXSGI25*;k>Fju_AnsO=o8g&@aVeqbLXQ7qk2Ue0F?b;io)m}b^_t|)04uq}ib zlUl+}2j;0*#5n)CjL_6Zx=o^h$cUCnR5`eH>*N;cQ3(Zazx?WphoF6G-5eN#Yj=a= znHN?~s1%o_|=CiG1g!{n2LPCW)tRQtHSYB6_YpROm!z>~BuBRqQt#w5Dwp{1^hlz0k& zm_XhX126WX6`R$XFF*6KkAD1<7whZ02BL?t;)>dYn?lT)S^$Q5v~zf18ap9$W(Zl* zU-Z5pUBo60C}WyZ07Jxl+?CQG*^3X;Kj1@%G_by-1^`k4%JG}wEgdN4ZhN|!PXz;V zKF%lorHSVL1F5FyD>_Fw7=*pPRkzs(!jWM=nP1vkPS1ULIp_67Y5tUU@cN3hmO(H3 zz#YNd=_(m5I;r%r(N)WuBDxM@%|s_E zv_*7&il9pqgV--K*JfvhLZry7*!Dm;@nJJt!sJKA7f-vRNymFfS0{w0IwHtM5lXNR z%F~r(FYnp@v%P0@>_Y8h>28uy$E~}6JUL8{x_O_y8T1vSl%Bbo@H#i2_CpyM6IX%o zCOtR|ig?JY3TqnDX(Q55pi&$`x?sQpyowZ)1vIvGwl`jR_G2IW_@`n4!%^A|8vtnV z!gki`ngJr)re!gNejUvAj23Vul8NL7UOObSb_6bBY`R#?^7O>ZGKP4s!#<*eBe0n%;7jR*mUS#hTgrzGJw zm;7+_T?s0;z1=i{ve(_6EJiSYOsMvE3P~H^kTFb)2|W!5&hay)umqi4z#i zek-#A`ny{hC)L#Tb5R6cYG~+T0-3v)DnB-(*#D}zIa}*lD(IGE6HDZx7}L)5@at`- z%4M~ofz)AnQPPP>z{6i^UjethKjAl8Agte}Ruim{>sERKkCp8%)UE!U*|Lpf2p+Oo zDACj*1ksIbF7xPEfkg1&_?W)-_~6hQF$o3}kW|IXy1!eSI#6Q+%LdA5=iA@pSE81Y z;0xXBb<1CKhF~Vs3-8Pc=v+$RlLy$!#f_&HCE1uWlVLQwCd(!+K|p{=hN(qH{HOfi zUfnp_D|R1kEKlj2r*`LLdE+D{jN519kH{MRTrnlm@(IluUATZQS^mUyF`rI1X^EbBZ0ukzG<{ zx@y5QaB6bcBtPWOey*Kv?xk`tF|AfyyIrWl_!u^nq_el1R=labt))$CvLCG!;VX41 z4Z|7%dkhEGM*S`-d@evW^E2a?h=#8~Z)+o+2>BQ?{&d_o0^Mv@2s#e3)6`~-o<^OkS$DmOFT zKb=`#V0sROn&@wDZ0zh$3J5>D8Nz{v{$AmR&!<(w1a+7N*L`L+xk>rlJ<K6at zVi3K-1a6Rg!^?36&FKI3who%~{cpWLWi6wa& zqJYmozX%k_CG|nOmtLYUK>&ew+YD9UpsZk7fnVCMNP_? zdorf_pHTtIY81_9yR42~buj^NHzz6$X%-R_Wjj~Z&C=UebPUk9nWI%G8x9u34h3 zYNb<-5FtJ6r2;4GF6F{L_m=0V;GrXW+ zajD;JlmRb2z>g;Y$Ru^x!~}-B3}GFX_@(j0Jb2GWW?hn!DNahQjF{^^J8)(bmrJok z#;t2i)p9rmKvYs&r=DHJ{NPYA@LxuCj!u~wkvNN9QV9&^f<{@K6(<>*li(Z#mSiUd z0_`W_CUzWe$Y9bwWN=S(lVxMD78}lAl_60fG+=LU7vA7nx(c5;N1Jy2dV@lA*LHVD z({*a*UO^z@DwAgVy?<$EWV+&zqb{aS0}O?j&<2n$0*qk`DAse7cS}e$u!PxAoF)|D zewd8LG|^eVDd4f=lDW{BHiR`{RW?9orUP+n`06qGJ)vA!rzO}wq*Y4T>F;f(Um6*H zONkt^JUQ4?6A(zsUQ>((W}w8NXud$D?|qcW6nLMEjsU>NBybYJ+1b-1 zSTKqy<4hu5ft*Dn{;*2T)*<`o_x8 z!9J!pO&b;*B9SvW!tst1fQF1skNFp%V){+yjW2|^;HAKJ9?QoFckO4A_3E?9R3^bj zlo=vvYHPT1>B6%YE?&CUpoz7s=$Y;7*IeD+IaE>kkabqrHSx=+QR1Z$ctNkoPa7v& z(xGyJjCv4pk``o-qQbF>;F^e}Y|ckrX~SRg;J0p@;j$?iixQq_XhDdnU{G zhjJwAA#jQ4RbBEl6N)ur?NE%T$9<%`&DBATEwdj(UJiwPip2PPiRLT#K-@4Gi2E2pP229qQC9DOUl#nPp0~LHy{|%MnUQdmfkIMog#_e9LTOP|hP$3q- zws-z{`vp9KC!n0L0E2Lc#&<|rL(18nZA=WArLaCV($m=uVFWBl)>anM2t_cyamFpl zoD!2h>>eQZ(ok58otoQ}5-v(lt;Svl4NVo}#wg}oHG>~GEto+$Bt^In%Jmt?l8aI* z^XYX`|8b4*BuJRWWpU33&MdP@slPz}rT$xa$%G`=o>DDwV+= zm_=%^Mm2EjaPLotZn*%SOo&c(6L-(A@GwSO!EoC&Wcv7yPi}JpnE~Lq$m~teNK`c_ zRPyV)Ju~hzu>vPzPfm8u&ryg7xQLgqpxhQK<_n6ASNJY`21Edtt0F2-@?JcPdRzXb z6CTboDsq+s?P%)^%*3;0W0NJf(^XDj{me`pPT;^$e^X23-w6ao|1Mf-oE5x3!pFBr zLaGKi86;*lL^;Zyiz$Y8h-l(PQU_p4x?_lS1OxOgo(v@vn%8M@8-ky{9mZWO{KMUi z_+S_aARxy~YnP@yP>yJ&7c#q`yT7Y*U`&;GxWD82wa&g#H#*6bG}x1o7)fzFD}Us^ z`Edl8PQUC9o2*7Q5(-1|>do+EU5s!Z$P?A zaNX>fD_8{}5Fd+6#llx~3rH}tx50p{Pn^4UrLlzwhstVnbeioP&}ShSq>j=bK2q91 zA0l5adkmyZ^08S<5TuMZi`xs^d}Gkw0+=yvi*^g|10 zyoK;g=3oqw=9m^3Igk>d8TjO@W3@+enX+L_}KlGv@S8Y zG$Q-7CD+1f8iGz}5~vkcFUk+Amz*z#fW!NOnNnG1D43OfOqNtfW|umqtI#8=!S_DOY&-$_w9>xGOH%uds&1LJiw zmVJ80{Dc4^!wQD$+`2Tgpl-CYOqFsmiu&Z80O3$eM^~=|+eKCca*HZ9b=NO8@iIY{Q(~s6&WTUO^iT9RMKHs1;g(Fa zHFVD8F6abB-H%J?Y-^Vws!wZQQm3Rv6OkI~Q6^k~S6y(1=HPFx`blw}9#^Q`T$hxq zi_m5I}bzT3YEe@!P%c+lNg6bhXGUtN=fKYTj8GMMry-o*)bjT zBKBZyb(>wo0`{&7!l^$_^*+5ugkGiC_C$Obb&bAlj*0yN1aFOtiTneS-V3Ol0Bti4MQWPM{(S0CzM4m!T9Pa+eS zc=#CIj5)MVrpq;d*mTJbEepUBFXk5sqzGtPkWXhy4kzxjW~f4R4N{R}aA%Yd)HYd= z-}My}`6}_w>E=>|lXiE^$})1=S;6lrEUta^6wp+Z$iOH37V>~{G`7mjO}vI;^sg=; z3&2x4OqPi8nXGc6@$k@#hYZoq%YMZ)FF8HX+T7ADIU3;a*~InIg8mRml~H^sBDEv>4A#&3<=}^KoUwe z(hF)a)#{b%V6S&OIofB$OX`pxs-7A3)3iaNrbn$isU}zo>`C5?b@t@&JaS!tJ1pP(JEVBlgISpTe7S6>q^QnGldr?kKJJ#^#3%W&g+sV|+^~Aav(% zfGCCmnK*o*N$TRCz*4b@uizi?HWOo&#l_4eWjnm6eAbvbED&6%*c^^4depCyv5g(pGgjdac49JQJL|-5QBLxVoPNWUMN_2j|7$<>?-h9Z!m1?wlgbv~r00h;F{diLT zEW~{g*KY!rk!hK?#_=ImOBhqf1X4?t*pHabKn;}UrD9-9)q;R^Y&tbSC{k%7nT6Z0 znKuQUB`WC=x^DN)B<8dg11gYR7$~1DIEEeM9h3Cjx(N*)Vy?y?4pRoj?hj|X=TwsviP>)G9|oBP~KI6#N1WF&oZ(5P2j zX!6_CY{WSZvwU{TA9TJmRsjjR0082{HslAlE%;gCgOeliv}vPJPC)?)(*VHgw2E+) z`?Lc=>C+#YJ+m^VeGiXF0L~R>8pepFOcv*K4GlwQpLW^V#mIo^y5Wn+M2APZTDlD2 zZ)m!4)dV0GA)e>Hur2goFbg1(!oI;8<~}nOKp z6!gEq=oBrwZC2`|S)<3m1n`GrbZR3i703@OQ51^E`>OhlSZ)T3Mg&ux=nGlZ)sGh{ z7T%Cr>x%&BrBBofgvyk^+P>60Z2MBOm`7um!rh=Xjy#bba^ygQUJU%IYghQU8I2A;oh@t-y4F>FCU!o_s zPLAv`V|YB)wQcEw8j_+KsRe3COp`Z>A$)4)Ju(tWnTYYQ?;lEvo3^7*__JVq80MphHJ zn9j^--^!X8V=tCnMZUo!^7RfiO?Ev--A?)mm_<_tD!Xn!2c8-OC@L(>nzz9h0Kd`o zV)=Ir4X!&p#lA?#kv)XzFrr!|&%y*U9KEc=^7ca=9r#U!XWaibct_&PG9y1Rbub0i zZH1q?sJOERAlnl;jM?EX!_Bd#EZD=xrU~w=#84I4vTc3LcYO25Ec!7z(14IiKrm!s8OgV~fAirz?T88#_BwBEet2V)r%08uR4Fgr36{?F zkm5v0LXRWE5WLt20y@hl<8kApqFqhw|_{(XTQ1np^Hw~3i)wQhlYf9Ge#O( z+KnX|Q+N(U@$bmxauZpA^IY-9d4p(c?5*Xr2yT45Jdubt`{S84y_OsZC(n7SB1%Si zl~B9DfTl`U8rSSK4DJPz#kOciU(^nXabszN%xhzPc2bhz=?o8yQt<-pN4L_(4ZUf;r`)TaY86^u80&2-ZGa99 z^nzPe?pF1OO^g^^kzvJ#@*>kp$-m;vj&5M*m|nhs0U2KjfcWUOxUOk%OpmBG&&4^_ z-}}!QSh9`dqeBqS!}bXn`On(wy*SK-wSZ=BG6qM}Ca3WIz93j|i8GXnvjP!8?XYee zzqL#|AkWo4AftgJ&G#DfPD36Gl*~X|$++C97i_l}dg{y@$hJsJGj%%N*vXe-efr-t zMQ1pIeKQn93N+fjRs2N@TJHfRXmKH5!BMcF_?AbM#i*W&);-rxq0!umm-$deI>d=ri@&#%9$ zo1Y_g9-B_x()p~d?Wyj4gVSCm!IWaq-6yII{%!P*TaRMYVri?xg4L;RpY~m>McI)XQSFenme#r+Wi=jSLp)AD$_FzdidW+RylrI`2@!H=`y zS(%D!UjA9+GJ$I;OJnx%sFpeMq2AF*|G)${K4^3#m5wBycR!}IJ~%?3?*Ye2!>UyT zyRE7>&R=OAnd2KBMV0zhfk$i{>d?iB`WIP1H}Pt7OG^1ewT6+zLcNsL3H=s|uS`Qo zhDfxU$QsFKBV^4MNfNr=&c>@(Zw7q}x&b_<0ZmC12U@TAK#iu40w7uWv z5;Dv9?(JhSOax~OGLeJO2FeEXCddPkggzuflFefMOI|Lk!?(GzT*<7q9C6R8gYl1?^OpkwXJq7r%HPyP=QIaw%Ye*SaR?e=f4E6ht#{r4k19;)r>>Gn9vp&_Mw$C6Yf-`xv8kQ;zeYZ9)AW$XLh_1Xh_M3M|eGzw%HwFp71MEVv$EE z61j~8NwtvaK)2LEradebb3ZH-8Egt1$I%BRiVAEaB!0P@qdGP?WM_bAGYRf2x`M7X zf`$zQHK=9;{c-%vJUMyzKkvR;H!Y@QQiVt%_6&5GLohYNNs>=;gFd}XKY3pYEGl^F zuXCeaot+IAQ*!Jao}?WLS(9GoNy?%KDV%FDai15QoGz6%#vu-MHa4`RW1_XWtqo5} zKuPTo@>A3x=PX(J%qzJg^~i+0-X1awbu)REVNvGBh`NHaf`c`3?ngcnnqhObZsiLb zAq+AFm+}W3Q8bEZX^Yz6-krM-@7=w7X9H%+uuIwV`u@Eq_wV24ESLubpx&lY3yW7w z?3CMF49^r=b-60pL&*zk`$v1acqT`T>{E5E8<`+O2=IE=a6zwy zlX0EBE$0abMM5b#gQ48BxavsdNJz)WltQu#LwS@ICeAtG4nV*lMC{M`m%J?mUM!vl zx&{q`#u8-FX;4u{6E;dsbU^K3#K2)MmG6w)qf%Y5OJ<<2`N~!EvIZt+mbNprKRtw1 zDwq+$OQIt1ppZBz3>f1V61$q4BsjxE>7o?p49Zzt8S|<>hi{x&Kws@9=@EcuGP0Q3 zK|IDfh{wSMqm|IR4z}2z$h19xAmt(3a21%hb~7RVCix-BKQ{W{@XiUZe{lQm{fGA+ zyzuz>#}DndMslS^cxUJ4qnDq5e4jD|@rAC2VV5^|lsP<$DeTl-HA%+m+Dur6z~f!* zOxsC)O^r~$GvXxyARaoWwJ@}%2QKMXxn^J>HRXyq{&^6H*A!Rce*vA5y2+!`;c>bp z2u%iF@^TyiWhwEPh~V;gSz_$!LqCG7#aTqW>DID|`0OA(lX;!(A|IOwp~T>8pc6Uk zluES8Flxt29tEH>ROm%__IB<2H3SQ3 zFTMczI~#TDr*vyahc}OJ-aO2g*q(NaAZ=^!*4_J$?mu|)=;8B^_L&bEvaW@#&7=EI z?%ltmJ216{OgvZw;=tXK@q$^wBT;eFjuZv~pX}VcbJNh3X`MswGudGkxqsQe^H^FR zG{uqzb>l-@XGYWT$E-b2-4G-bzAnXWt!>i4h_|@w&S`8k@-OU8UWElY!I(s( zTS9=zYwEIm3UYfN};j_*!MC z_43o_&Aw^8k%hT!T_Yl=)UpbsPK0C5<;b#LlWxY>qhs=EOgkDdnA~_s!vLFGTDzg^ z+=OC!9`q|y9=UJnvm}kesBrLDwgR1w4z%33dRaRr7==`{dXfE-(mN#Ll=!*a5GXgK zwmOx&Q7nI3X0WMSWn+VWf@3Itv@3W|n7X(|NIoGCH0723ry@i}ZelMDEQ zBr@pGZwz&vU8ri%SS{WPNZ+ZZ_=3-Ig-0jOR`d`5l-5O};rn>}^uM@fDPYXS z_h<7x;6}Ev;)G)=;n{OhQf*)b@c`6N{54pt)>@{S-Upw=4fEWR=VEcdXI`epTk8vc zF4l}&k_}^XjGXXJ41G_sOZhcP$X`lMmNF)s)WUD@si~kY;$kv4iA^|VS5YDK${RGg#%nDz{qCC{z*cNZN)ePmL3N(!Am?T2TcW7R71$;%Q#(#EV49R)Kv+-WRM zOBesEV2(ugMR+`nM6vzt_uB+$Nr5~FK|vb*lO6+P+0pIequrfbx9>gJ(Eh91qoY;i z&v1j=cWxdZZEuyyifmBR1I>qh>z-xZ(u4O0Ehee(jxumyV|&FbKyMC7#+W=EPwuqh z^P|p^*_8Ew-%`7T9Eg$++Tt`FbSyyTASKt%VW_Pn&8$Ny5R4O{U;If~j18`kdPE0d zr2cdQf6iL1Q^ZeVmXeP@V^b?1UOi2BkvpC9XWS&EgaBHDQ}V+B8J7hwsQBgnmnIbY ztCUhvKsah%lsSf4iw-}M~WrM zk3N!;4#SWK0)&UN^}bek!riieit`lHv6aC>jxhYCuLT>)YF&*YB^*o@KNBRBxspy1 zYFM0H4(#)|ylC)ZF?*OfdS>DTe!{YG0U271_R9p4HGdhB!N|w;4{r>{p{mUl+q0$x z93HOB(XiTG+O3}j1oZlUoLheU!#94oyYc#u?hW;S>ub+9k-HK2!}M1x#;EVw6;OO07ImiOFYLxbkN|k-L~m$?SnvFAI{WFz813xTOFc@=MCut-M z9+(FNDz9+;PEWobIsNh;WteBYp@>P}&Qnk!LJkJbjdawvnnOU;>7aqY)LHy0vL6{O zy|uc2kIOfzYiR7u!23d`(DS88x$J2UTQXHA$Y&GyGJO-+mQl&Y;uwwA%vxD)s#?Fk zE|$*ov#A1!4`Hcw^XGYHnH~ZD`Xrl8!P?E_8)#UTr=4QL0@qg>ua8j}GlhuZxnxMC zqu4WLa~l+{+nh7s4SRtL=NVvL7lo*Ex7hy2mKxK(dAU}p9Br;35yl7lMrDJtCC%fw zu@~R?w;vsB|LBcJ6Qkex#*^L->;=_hkMW63%RhSK3AcA3=! zT2Z&&)H~r4_X$ewSPhAVp6WBG)XLD%JH<>-Rh5Nd%Nvr_8392kj@2KZf&;2&qMnQ? ziBh_QKX3vdTdMVovK?ZRCFHd9O`a5l;7QM{)*+KgPAKcuP%=9pt_6h${bf>;|E{P} z5;9u>lZao=aPps2N%#Us&qTbc-=O{K&WgsNW9)N|6Is8YZ8C#mu~RSgF2L$0uDvNb@{ zWRtWzD>10;y4}z2QGo_9WP?svGbfOd#r9;%G1&!mJ8ZeGxfz{Ek(eMbcg8%EZrb$ZT?vb#76JGeEf#lfpjee@%r zeAbqNXRo&Pn;}5bzxnOc+I#Oleo(7@adh(CcYiX6z(9MxU>EY_@N>1=R}AA088ZF# zZ@o&$=;YxRN(=tlvP`rJ)n33V&7)Cfa*ArD&{2y*EIHxO39b{-b+NkdhBKkms-i%Q zd4GV-6QOZFLK~p9RJZz{ti@mYadA?Ndd@1!KHw6~IXynq(b&?9VpC+`d{BYT1MlUr z64G+xDVc?=GYyprfRHiln5e(DRxzZd>w4qZ@@&6hY@HL!>*QK0$uj3q!0|J}vsSKR zuxDhjFfn3y= z5&e2G|9q~$Py=X^V*r`*DDnWAbCes0cByb=A+yV(mr#)7S1c70$J3c%L@mwZZh5KmA(b5<&0mH^2Sqs8s|iSe)c?+1{zt z&!m@SzT(kD+(~AadVqY|E$|dfC9pj`rDLuQ4r9ORau&k$Aqd8qP9-JT*v$Tq$VJP9 zon!n1nQ)#BqKj3#Y&`>7Z_}zYJ>J_!pkZ(p@4diuDB9nzm}69<+L%WB2%N|~F8>vdN~}nN?`o3{Sr0K5SC8oqvm$SlIKn(7UkZ8z z3x?kD0KnI+x=lP(4s#tc7<83+hOJ_Q{|i9^>jJ^4;04pfwFr$eu2Y~EVuozfgICJ# zS%6gubos~=LVWFbHj2-THeP)CQ|B&Uzk2CfbHA~VYdd>KJKsI6oxc0x^L)Tq=>xd% z_cP&kILA<9hB^4`2HsF*u!Kx>cq^+HK}_Lh#G z@nsu~at07a;zkg6yp$0I*^w13xbn`2#?KThHzA8C`tg0C&or9*H?GO9mBa@vI{9D> zI43h(S6$-x(A%l@*u}^jsEn0mBuhvqz9}5!ruef`&A5c2W1vO>r1??zHP4QavM$eI zucXptUKjI;1L1ZuV^Enuv27-DIj%o-q}&34IeDh5HJzoXJ?4qY>YF>d?VuOe>?RP~ zo9i!MxpvJmuj~+A`djTUpS{2NLap}Im8Hke-=5YWpVU|govb{p)xMl6kc+oatNnW4 z^WR2$@jad}@b4pr`*fv;96xnHNR4i(-URj!+zBTU45MHJ#A=;&^tnltBXDs%p2B^6 zUd9)ia8k_g&@)qj=sAee4Rw%;R|>_WLtSkhEm!ST=`gbG`?3c;k&3)hSQ{j8*W*+> zY2(isNyP=HY?s!3w6*4~Skg*?y0}yk-1_P55s}l#%$c=y50$;j#N;=N!f&}=L3gpo zJRZF(hC?PWai%Zg(qg$A%0EayywxRXf2Ah(!SnFV!i%amHegDwz#P4wC91$5BNwO` ziaA9yr{103W37o3M14{4NGwHGE7@J}L$iQ^89=qvOUYI_^nEb`8*Y}KpCstxxjCtf z7nFBY6>w$Cl7Os(ai>IRfGG(aMh>NAQdv`TTa?=UjNrFAWi#f|;zFx1n%es(vjyVq z+FM_EwEI%6_Vvw;mtKCD*8i}V40~qoG21YCdc~p+R`8nxn(jltexJRI425-=%ESs7 z!5p1eo&p1|TRcsnLl=@i;eH>{QaO^FobU^hnLrw(X{#7rcj{!X~)rhxk_>14Ui z)XchNbTN-0^piv77SI=(YKBE|C7m&GI`6><*a}3Fc)=e`P1jAGj*oMc{z#DuP)Qd} zY(To|+z(0TOS_=RG#A0N`w$`l_9_}OF)RZdi6&M_7nC`Fy<94$oaFm9K-jr~3W@er zcf0Netr{a(+>s0`YH4e2eEk~sx4CmzF<^7^?b=_z^yJ{xIQ``(FWe)bHdfd9^Es7A z;Gk@GEr=_gKxjG)eII}6@=c{RZgI8LZT=re&&^m#=gkhivPTx5_CNnRrp31^oW3gBoSm>M^$r;DtlWk3(cA5v%zVK*;+qzO@| zjilS-g^cVZpIL3ok*7%c$QkEe?iCSjFJFQ{>||J9;kZ!l&zawVJ3mpq`ise-u{BNE+1h) z$uF}pjAyjyU0&7EaQS-2NW?G8&)r!VuReL4KOEVw)jl^gbnm&N!KNEM>o5(j+fXb%IFH}&3|(4r3%Qgb z2<}Lg7|*_8jnj?w*RD5#g5#O*m?amT7uPtgujv2=~9zWcdCcbY2&)-bv`OB|9+JExmak2brZr950qgP&1#@krmfAQ73M^7HF&aB*d z{@(WXz311fQyO#9;DaTH#1@z6j7svwZf0F_gT?q~(FJpcms{p(T51;|qLn^9Hfy`3 zArY|%l=e!hhC?X)rrRR##>q%lpeud^7&-m!sev?E9Oq@;0ltYeMswHg*JQK7A2~fb8l{yPP=o3Km)NYa+ z6iYo2l8GElM{AXFF}n4ngF2G_F*(Kc=)^2!O~ z3i}Flh+kyiA_~PB!+qK#kCMF+0|J>|A&-lUp=a!#)@X|}m zOYGw-Yj^G+EiCT)<)6OznOhRc=U;fZ_wa=mJix`BJI_BjNV0fv|Am*IJUT&f4ZiZq zgS{s&9u-TYp3Cye&O;_}P;7qf@TE^bcl6}R`r`V%C--)??>*j}tB$6AsZgU*LAtPR zmaIF)9!gYBaH-#NjLf#wA_@&=CoWI% z35YdfoKP9D@j+n;Z$FjQC5@LXHX0Zm>@|E^DfLQyeRE5vm_W(XYw31-XGe3xl`A${ zJ@d>{#m8U#m;e6X{_p?(KR$AyX>jcJ-%md`zkXW#*7W9YYd@d(dhNZb;g?VUe!FtB zR(o~!t=b!-J>ReWrLFn%wcn3ly7F=r4-t4$| z2l|-P8*iU})su${m+t)JZ|}YL_Rk)!ue?~Rz45zu{<1UnnYZ41=hr{1)gFf>oW`sE zhZ22fYxl7HNJ%AqUy<5}uL-gX@dEoxr}#f>a$2o;4|VJAyOO>|3^sKkC8)Sg`FhDX zr%gYNGWL4WR)S}SL#RZ1_2HuDrb!QhI}1C9C&$zWAz*fI-96$ufNcn9r1RRj%S~O` zZ#vv}w&m>gGpE@}6Sr)1i9ixoQ0NB-f(XJC;3ehl=BCgw0qF4qq0r&9AWZKoVjLj~ zQ_2UFlPlC%eF}D{NstjGGmgrOG}~k7xM4crux<+itd5p9NwHcwI=YFEQK@3*1YWF1 zFHXaahD(>uKmE+Pr#}9%k9_Q-pS;pCJo((;Prtajds=&Ke(!g+Urv9s_TJp&XHNfq zZywF}x#hpseq0&)VePGs)-TllFkOHB^0j@(e*g!3c5t{>``JWW&kz3g-dkV$RjoEM z%{zQQ$N$--7yqfY@#^;RFW)(R=f{7n)iw|IUj6qR|7%7|e78LQe}7bL2qgdUZYMHp<0w0k6=}~oDC4;eN9&zI{J|dQxn5yJI^+q8N_xagD;{t z$Rx-NIpySI0>wggc@dWpHzv7p;K2au`>CL$13q1C?1YnmCM6kc8FP*cASXO!0}B~* zL?afyMWJ&+zyEZ1lMUk|6Bcbr=vo@;4GW;a8>c$TwEc=cp}sD|$eSClT)TSl%GC=` zee&ZU|HRYRJI1RI{(kzU)t%GYcjk6~SNp}(H)`)qkG^vH_vdEsI^o5?*4`ZN`$6q5 z&5fU{{bA}_-?huv4?j5m&y9@MYCo@Z^yc`#@#|V`e8#0MZ~y04U;d}s*I(Jbnd5)c z@vk53zgph@*L>s;j{kvB?)Jaw_M?hJW&iKnzq#>pdHiqfPkq+$|HkpZxL9O-nwXlA zH!4-cEn$5l>%{FQzQaeJRaRBwm`4QUj*eCv?;|mg!HcSja87?GoL`7VbJN+J7m*-v zMCwa)L*_49Xsn1!tEUk_+mk6_TFS(Q{p0PMrZT##+j~d%9^6FArcpxgUvJ~3>o&$J z7gWa1_MB}!b4o!U^*6O#GB>%U|E51n5D0Wh3y<8%AqzbbY>70^)DCo$90*Zz=47&L z8e37KPqyGs)8`vCWyTX4L}Rw;2-l}GtJTDnaL822+@UdGT6J{ak7!vQ?QgBWeChnN z#rYp2b{ZS5Uu|e=tiO2SyuDNP&8=PYzp1@Gfy?>j@~yXPUzm00AB=IgUmX8Z?R0kJ zJGI}huW{e=UB9orIc5UE;O4h#wZDAlt1~3_WO(|2p%+ueWd7ytIgPySZ>!>vu< zGWhmSfBYFaZE-Mm^KZ3xe*W`!YJWT4c(tgF{Qk#p-1^+RwLic9i=uXVFT$C`&k48E zUQM$-`-B00Kj~GlHFS_i%Pw+!=BbodM|D-IjbaHIO7?7fqs8{(d0Jq$kkqsIp()@R zJAz2l`>R@++>0+E4=pa}iPwDP%ZPRB}H~n{w?IIpK;R?}U1Qz44P-8>=jryzCuU)-ff9+~h zo6(ckE?=SVYwPY`{%!4zDkkUGTlfE~_LUW>-s5SP?#tD$)J~VCzE}Ih&US2|`}ehf zn^pxL-g4q^edp_o2W+6V-CgbM{%)=I!!O1LE??hc1Hb&y-)sMQvi%)4@cQ>|Z~ug+ z`2L^Sz|!JNY~anGy<7YK;n8=0`I9%1Oj%Ng?w>mG)hqwN>BRJXqOlbsf z-udFYwf~F}yzoH3S-POed5vV~=}yDWu#XtON;C>Vo2$+uRDDv$^-p@_+yhD@N;*TL zu&8##o@5AFJio>x3b>s$d>-vyvGRp@5kj%(Kxj52{;101*po{)6%~nmGCS)GwMgxG zPjYFL1ImAPM$E0OS21^!qh%t{O)FefaPdf zPpc13-`*RjyjT0$^jm+e{gf;C^T6;6r|+Dcyz^I{;l0{d3?YC>CV2MguJ*lJ?d2O+ z#DHfzeqZ}_!_y0Ysn!15|5HH~OahMoI3Ay^)qb%uRXO@g?T;HX&%Z4hSo(SGKOe08 zbp0o_Ki}CncX0nIZGEFJzdOWnIS_MUj`83w>q060RLSdqyH^)YPFBO z-WIl&?%{qjL9@jSE3+&taMen=W;w_+h*HKp{k#~7c0x<3-;1Qp3&$NNz)%w@*xFr; zt?27to2zz@eQ4;+NxlybapGhy7G6Xwh2ssQh>Vw=>oJJSQL#V`(K=i1Ds`s>p!d$1 zj{C5>SIb$~ehT~|U*t?-niNvf@C@B1mSl3G;ww+6S=Ocw^%Cv|N3gIo+I8i^`DZ@% zkxzc|+_|Toe)`#qB0*DgTeF;?rKzc?=NjG zvw-6-?^EsX?d={NeYG@!?+HcQpJoClNZ#Mn{`P2L^FL~}=K%^A=b>QAF$<7RSd|6L z_P^A172UusAdf|O^pF+)Cl%LqWN@+en>2?QW~Jsn*mhM!Y;MtwrjnReG4=Irb+=84 znB_JUBZ~YAvlR~-VDqBcfT}b^2NAkcXScK}V`2?Xei^T^{w`7O?F*SvrD%O`uTyFn32nef2w~%GwUML)< z8WQp+3R+VXyk@F{Y=(p+V=tsLNJBQ@KFs<#E8{)a3=?DpAAkDkPk!o?PhYOTalN6j zx#9Y?YuB$S2e!1dv4Xy@{{4L;xLH{~t$kybANcwFo3%fWj=yyJ&aIpL!1Gh@)xMhc zf2>RGwDz+~SBFUON^@xiy??0vy8e@Mtl)qAk66L-?2l{j-kV=#1v?81w_*iLFR+5m zO;+$|qqKtU<5#}xm*$Fz+o}@orD;*Jqs)6svhC#4NW-?M-2u<8WRv~NC(WEmnLWwOPi?m$L8n(ju94mrim<(BRa)WM5D?67~QlOyR*HuYoux*dR`)%PH+oe!#L!W1Qi5L z-!VT2mD$-lG&ay}j=At}LbRn|R#WJo(v?ZQXT(N3K-|&Z*`?2r3|@Or_$x(?YE@p? z)!vgY&~@7?>RWww{=C7W&z|!?d#)kFYgeycyL#hBW2*(3^>$=S2{wQ7>z!}Bcl28A z7h~;RKd=2{xAyg4{bT+2wQo&Uj{j2o!k21aeCzx7Yqi5-@)d=0UjCQMBZHX1&vwCt zZ?r7FTYKeN!@y5#wWpt5hZL_|d!<%8X!CbJn;Pgl;X~%;c77{4T>r;E*ZSUjvj6%& z*Is-u#ijguAx(k_8nkaeU#so@^f%vn@A$RaFUNcOeqQ_Oe(f8- z{>RSmYp>1D-~LPOU%yiO(px`ZA9p#Tq%XmYQ|?Q;(+5W{a)Vo z$-{5$!J|LkQFVMR3t1fe(p2>|reqr=@&$Br?Hp?|CyjpF1r;<>O+}d`P^O+q=cuJr zKiGuz23*JgN7a3JS$bAyqHpn)Jb%}$b?>@s=GtRpGAOG*DXsho4pp{qJ4wbb2` z$S?$9lWhdpCJ14$JvN#dK*p%+{+`p=v+h!ZT3ywr&iUT`?(pnqKYOsXwz&BBRZTsH zk9*r2>*_9?yGUI(ql=>ms=NpP>E8a~;DRPkLY|NCorQ;z>YUUYO&bGt4%;Ad!fW zqh!ZQ)JJ=)c5 z*>*#R4dmkk9g5$|bCuO~jT(gmW7+Lzz%raOC?TI}F(;DGf?;7Lg<{OfI6znaGD%D? zj;l>mNIITzD}(b#SFaxKEKLmA>fSfHcC?lmfz>l7jKhI%ZO(T!H`dnHTzv7x%G%n- zj_$#UIWuAcd+L5ny-B^_2*Cn-iEi(y;M1-`7Yxvse#(9_r?w{L5&=q%d5+SX8C)6m)h^y%&C z?im~&nAGJ~6n1y^_U~xk{mT!ooqqM5(>LjseC70ieC_n5H%>qO?t8a!tKL5S;om*F z^!Gpe>(k$>W7jLTKIQ0lr)TcD^ONJJr#~J$eDCy213go3ou0O~rXnm2IJ=|v?$gts zOm?;Iym$J>_|)DnPEU7t|Lyd>KYI7`FW>#`>HqrF%TR{<@%V48&dj(sC1is)PfxWF z?%&=rhk%R@Jt&byoDh9$h+;WrlnrZj%a8ZWZ+ljpAhL+;5;``QfS- zpS(?EN1})Vs{U?dd_;L=Bu7pyytlKt!Lx2@ZL04Yo}d=9dFGzn#02BP(dJNRdt+-` zb5(Uirf68(D)i9`1+*NMXgC>wAkkmJvN&XVz8L;I@l4GGNOq`Hk&+we>FK9I>J2ON zOT8W3cw0*voWoN|PQ^cwYP_qy>Oy6GTYGbBSJAWF)!oxApBvBu>giGYceJ+qABsXpZ?uX9$opx z+i#x!9zKy-3h5&@UmO~CcqhRHezr8+%R2sI zdM4+;x9|M_^~rZX|KMe`qEElV03Iw#5#L&$o5$c8np)Zw@^7rI^D2LL`O7l@l-x0Z z<9(&z>+_S@y&jZQuQto8XjZL8%!P))mCJ|g)T$=*TZ>DB6v4L!oz>1K1^kui;hvU@ z&wc#q7tc2|we|uIBgQhOMGnzZKg#Sle6s_ZiRwqAd6GZv0n;_FsO+{5r5YN;*oWk- zMVO=%D^|4g_}5Mp18A{hgWYYOZ%;R3#PkBse|YARzv?|6UD_Jy?NV#C%dk3Je(P?x zPl6tidBQ@2NNEpi5CyoB$ryq`YG9igC*}Il!ZS)23qaOm87yF+r$r(sT=x^#o5ZaQ zmm%$sp60rmn&ysl)jN_Rse9baz{Eip|W$^!4;~ z^$qXU{?Ang1|= zx3`Hu4iDg-98 zO4Cu^=ykUVBEJH2+mJl?M_G1w`I?sddEOxIIAY7%t;WE zo}8F5G&Kl@*8PvHNZP-2#YoN(6dQ8?`v}2~atpm}-2;7$tFijLSXNhC+h&hUVg)v5 zXTL=PJjMtx$>2s?XMe<`wTD|fy4pMV1Ro{^rqppPl{PZ$25%H-=TabxS7CiBh2+qZTcdkWuB@?OZo>m&{ZP4H80_!r>*ol;jO6Acg!}8$-@f;Sr5M5IVa|-; z-Cy5&@ZN7vfA#2q%IMcw!OyzB9xE6gynFweN5TW3<){Gv!U>-KWR?~De=vg0eFpJG8SpwQ_{?swA6AfQHOmM7hI2>B+XlVTRd<1j$4&NkH8#17 zZT-kUfcd$slZ#YuslPNl&{$htUtiZ$r`GH2Q#1{YLCcL^=&xiNNlu|~MA*>`EcO<{ zH-@l=0VbKyxi9vw>r@#)#YiMUgG+yM>+1gg(m+pl-{4584Wc$UHa9!k+ttz4)&In( z?P7e%_C8CldBL8xF0h|)m{=<@Zj%x4cm{g4Ki-z~Ls{(O6V`k6jMD}QOSx2>tA*9d8UUk4DbrntC%;ljBWKKg+V zJzwwAHY>p_rT726{YBzDjpv=wFn_Swqd%Y%pqpnq<)8rI+**s~zi#1cC<|I0r41r0 zXl<}b$hGb_r#8N@3>X?A*wNS3g$rXiYGiWb;9v>21^<7T5C_hbgh>>{8xax*O9Oo( z!(Zv+ZMOfoI9=Y>H96Gw`Ni(huX4o)yIL>#;^*sb6{q{J)-@>7M*2pRzfWc=?2S3$ z;H}jKIsA{l{m;ee*1!-O_-yY00O0n_^gE}&{odF2*ClvweNRXD)3ehLfA{ELzW?S= z|M7bZH-3HkoA3SjJ?-Z?3FX^+Qhc`ci{p*s-<V~Q&29YzyoUyR^x~U}hPBj%&wTu;i{%x; zgj49e^CXiMQ^hoq=jjp2C52B@$vyxRP7flzUCq9MuA+TKRj8u#_6^yN*k#t<+|kun zQTmkdFusWRW0hQ)3Z;Zd<~Y4|e7r_~aYSk`x4KHfUbSo3NRr__8wnVZ7QE?=zfk{w3}Cch{`a%ZnQ4*WFISSwuPr&^AAk2>7{JK5 zNbvc=5&6JNi}UZCzWe=e9Pdc*-g;9z_|ppupJV_(`2Jh}_~s%5`0bl9fLE8{0e^oi zq>Bi@ys~rkx2Ny^I0kUMEgksIrOP)ieN{X7`)?JecOE#J$fbgIfUQnHgJDuM*&3>= zig&>#b7YHqQTxw$*VfajNIwXn*SNGj7n2xg3r&-a(7 zl*72AkCKTVB|$iVNk2s_8M?3OvUfJtDKC2sJnI_i$d1b5gkPhx(t*JglQu7dP#7-9IldGymkmt6Y8t&YX*$W5_K~XePI4FBN%( zEEevh&CuSai_e1n3Ax!ekY;aD`t-m=5LCOcF@fR&x(E76co+ZO zy#tQGjJB6hYYq#O-*?CdC69RdNN?>D3;5;1N!bF31!&g~eX-#-7x1vQ-c=v%V*zjP zEiFFyLoV>v+G1S5cj5v{!#5>y14HnDyYsUw;LUFw@BSYPn14MM@BaVf|yy@-z)hqL8{Mt3JzFH@{J=>EdM$EvR{=|N zhq$oVJ%Gj1-P+_`R9`rIzNYf*Cti5*Y*l@0V^#Ip^R*2%4b82ceWhX~w2MF7Pck&a zgQfnyqUTX>e{YB7fh>U2@9BmQFo8b&f07G>cX zSqxy(Ho-Xnv9U4BC*_^Ih)fbUsN?3N(|>Jg{uGX0W8cL1f;0Qx^0o^9Df{<0$0u|3r!yN`E(?CTG_&@j-@f<1zx9u&?=8%T4gdm1aA$VvwRary z&GF?Q`|8*K;q*OO_{+ci?W6a8^ABHK+xim9*W2IneG}7p)%YW?iwk@G?~p@({ev4D zYx2Qw?amu9+5YOU-+TA1@0~t+sKr~Xd?7rmGE*j43KgTWsDJ`7y;X9~Wk)wRR-WwE z{OtjSwYQZYV5b%BD074g3OQC(K0+x$%-LGQ%f=F3vq8JOC`Ar7JC5fUq1TyW?v{K# z^I~3nVg$r+{^@7Vo~^8d%OH-K09jeyh+eD(q=S@DhOC$yJo7b^+Pn( zORZ==aR^f@j%3Cpd{~FD)wI40ewe~szee+6Yx971*3HYiCx;mn-Y0(RRXrk+Qm7Wh3~(9Td_cO=9-ddfq6f6Ipdu#;1`Qh#DO?LThnETG*U$TLpy?OfRwPIt!Eezu> z_x2H-BHdPs{)UKA^?+n{2D=LM7{|?48qo>**=Ja$zf0p&N=4ihUsK<`j0bA>sspe+ zN!=sq(w%(VwG9ths$Omm1&c3RoZFw74bsNl^;OS3cW&!^bz{F~jP4w?VsjUfV60^G zs5Jv)cp;-_9_-ViG3LF$Z!lG`P6MxtD-c4?zi_td{MlzKFILtzA#FES*R{FbrTq0N zl9PkP7v~(t41wKi7$VRjWef}JgZA+Sopvz6OgU;P%H1GZ9Z|?2{hyFH|&%NWX#}P1s2j%vz0UG0) z#r7`}$;-oU*)OvPE9M|V8Ky5ahg}nNf6nkI1L){e2z7RGr2Sm29TeqjhLVwnecDLn zE>kw22r($bI3%b%X29T0b9~P&#pC0Yfg0P?P+%Y(cz8MI(3ukFS@+Mr%V;eSOjRsHUos4r6Ei#p~Ct<`0asn;6jZ%!O3Y=|JYz(k1lccK@01B?X}e)X#EaCIcvL@ z(ki)n^VZGV*E61CID!ZpE>+*)Kz9}m4fTyZ3W*)4nDLf@Y5KcklRNiczM+jcK5a3> z{@m=P8#mX6e5jI{^4Zep-Ph(J}ul)1r zJIf0P4_`f+n7;JzQ%5}W$;)9TaCCp;X6M0`ZYaZsNL-cuN?l}>AQC!NyW-=&Zr>b za+~hY9O)cfTL=#R28}&wdzsD?w)tFP$&dA}i8MX@z55v}v>db~6d z`QG&UNqfN=!Y5xaokzD}Cfcs*^I0vq!Ccd^p|0lY>hqOVgp-DpRK0?Z@M})bZhIPx zv(iD4~5}YXrRa%24@@Ari3+F zru1F?p{nd|(GLitX`sItc%&_meL#umz+{zU^uL)we}}54xdY~p&$M-LqG@^k#_gBx z-n&gYGwo+G$kHIdG(2+q?hS*|_wHTSGe3Ow!Qs;4om*EIW|nqedie6Ke>wf1o9hP; zAO5=$EUsLA@bJOb?5s(&>nH!?^nZqKxqs{7{o9AeNX;hQ-aWYY^484cjqAsg?0VwP zt>ZAg*}%Pv~~ zrjP3qmrcM(jih0IqU7@TB9wM?$eyoQ=V7ETtD8tN&?}PT?a@oqaNahCdQ4=Nc7W8P z5X#j9A^Tw7Ma;9M4aNEO^TaB**|nIC~b zcdwTe_e4PLIVVrnHU(t{%MG>;bT-u0H3b`z`ETx@*u`?~`psK6Ze5c@XFu!uoM<~d zJQ0*&bZoRVDM`dHG-Tg9&_AVL@n~dx=fQ*9Apy=`eem+7rRA%4?rtN~jSOABer0B0 zBy?ghXkaP6|(t^U9rTyujMZ&ix06B;NYSOA)y7rq*ijKjMxzkGuQ ztl#|1r*B2*;^4rn=_Ou(v;@CAUKUmY>=o{PHdP>2A&7;jBgKyzklxu+G5=N7{L*y5 z+fMGaqoXffvl0{)0%O1ie~?nXmh!>LvA?=&3ja7m`PqIZ-N-mDUT1mIt5_XI;inOD zFzvRIbuj( zVaCM{7DHc~=y$s48lC*r#d8;{>*TGlAIo=OdCi^rI71XwQ9lokmM85u#2oTx(i&<7 zH1`K6IX1T_7kUQ=D@ykyiK7EcS5_uSN>k#BDx%~7cNzn3*J7-x1w{}38TJ_qFvv1L zQ&?_6yIKg%>Vc{0fiC?d&oa1oB&`b^X^wN7(UBev*BP0 zy{cPD-mWC;>gVRccmtzj{jHTxfBcz?ox+ixL?Q$Y9Ws>m;fy~~dZO>l5SB{C{y*<* zMNl{;OsJKM#v7;SRHTB7gYr1l3De?2zdS|N(psO5z*>6)10#|1=xxtDN05WiQ|Ot@ zT5mYqtC%m7##j~1Xux-ANAdrx>C5)VOpTS6u4?A2?Ao_?V5mhRfZ<|!ugM~%s)l%A z?=9nVP_c;Hp}bKDmPmzExUe`{>MaILql7B0+me38jp%c1Y!La@T#a;|COfSRrHTQ? z$&;_&zv+m7{|1t(A6V@D09PfUnIgj4>aA}+y#2~gPv83*>}|!*4DThJUYy!oR0>i~ z+S@Kq`LT@{fR#$KXco}8jlG>~-}%Dp55Eq7+DE=%RvQ~nuHDhgQU03$THiXNoI(ld zaPK&3_c?#DKz_${Q!X7C4a7QGO#Jm23@4Rf9E36PU_8QwYLCK-kpM8gL2;yeOlYw^ zTyt6%vw#-u)b1|->os;6hoH$bS!%7WtZ5x$=OzQFeU&#?au|%rtVn+hV5fp?!zSq@ z0b(iRKlCcN*l9LYX1EUw_E$LFo}%kZZ7ppme0GD^Hnep2YV{-&l-@!9dabUQ1c}%q zW)bk-+Hen=poG&8)m+pN>I6Ds{KAWhfiIhJn;aipxxTr$u!3`_AAQBD=j}ZMtyE0b zBAF5OLYz5!fx|WypdeDbg2!PH#=PacVa%fcwX`5y51Uc65#oS6tI3SZz?RZuwS$~y z%Csjpb@G?@uHF8z^Osrk)`iS^zkCpxfm*x$t%rB+JO4KzZ_BW?V&$_kfaKW{tvCQ| zN9mD#OZX>NoAY-pGf`&(*&H3*`p)NHd-#{m-wr_(X}tT2)w`&<8K;qjuWm%*Q>L?@ z9T4mbyGP<~m`*)6Q+5F5rrTIC@=5^r?6OD~2^1LCcs!C`+&r<6=+|a3B%J^?p3YtQ z{0IK1XT7(#8=yZ`mC1_if01r1!wgN;DcbNsO1*l`c$sW_z7!#OYnUV{1nCS#t*bMAKxP23|TPN4A zU%hna_0N6rwcSzJA~1492Q1)^7RhYI%y1@R3IqciOhMaQCs!XnTrDPl=dLoY;Q=gx zVOvY=g4))-_QV-IwyO5C9-~pStPGqXek%ZuNh-6m95l7E1~N!UQ#fb+am4Im?BZ5= zFHZO?I4GE`V<)Ho_P=w)2Pl#9o;bcOd%jS46mc&(|KGuwE(7fZ5$x?^`@~U2r!iQ& zKjDh~Ba8O-l6YspZeC?y;W@2b`iHUwymP~Y6#x16)rvJcwiU8~e@bjy#X;$?Ae%)= zDXMbBgS3h`@W`Z)uZ@Vp-@&zl=JV=dc`tNOj@3QVj400 z(N+6w;8a+R8jN8gh=oxao8STXAB(J$#c;i;sa2_wTjmhLH9F;_ijN#fAZ==8=V z3siQmKlt?L9&S^^Yj4W=Gn>2&upj;en9LG0S=q2PY^$3ZYOlSzCUP4SGa55J0kRm) z0)(t0(rOO8Rjhmx0p2GZ*Mfp_;bBh)AhHV}K%|^=YH5XJ@)D4S56GQhFpw6To>1&6 zX17oiJ-`6*qUkg2+6sQQo(zVE#Uhn!Pzrgu~`uYf1b%U&WC#tRz~jg zDuuXWWy^dW1K^lL0a#zZ^4J1?b=hN+ibwCwu90?H z5%`NywE%$#XRG{(2pW&#UD$uN4khB;dD5{SGlH!+EJhQiX(XY~EiJxn)(*@pwe9L^ z?JUWKLsSxsAf}os@TJTdatR}oAW3pO^K+xL2v80f8YzMRVY(x@sLfSXm36J9+h6+D zH$QhJE^gM&Jx`CbVG6jLK=+-`e)9)!e&q&VlT2gLBK4ep`YdcEjypqKAbuXyv@7Ed zSn07B^Eh`5c4_VQ8{hlkx9_i)ppc( z9&d=3j&n?m&*1gr3K%}tQg!jd#me&+8bC4Cm1kdk?(EYq)YVJGe9YHoS660~!sBy# zjOALbzWTy*PkrD||M>s?^ACUYnP;9o_x$rOp0DmuIHWcbDFn*#EU{jC-{P5_E4S|4 zx_pdcR(6MaSTxEa@FSy(4cKC02?e&G_w7a`cXdD3XI-(h@!h=H*|o6XK^1nToSa;} zdh_0Ww8%m4XpLI0taZxEeBp7%MlrJ_> z72QS*(-53PL+t8H7sd2twjlELP;i-HdBid;1k@`R$G!;%pry64j$eDOO!{Qeus+JilUYEYx@vxYdTCzrH)u#s#rLH)`Zh*S}-zW(ESsW z1JM>P%j0np#p3A=DQiZ?AW#Y>6qzuc(1u4Pg>eNcviSA#z zY@ybb>o@P-fAz}2S~0jXDuU$j?dpT{2D{u`mmvMLJQNwh(&HPKJgh84=lExG(eW8= zth5k((_e8pbrC1*GR^6C_5zTFfB$d&z+!OpdD*UyLL*o20MM9gi)S{XpSC-Rlsz$` zPMRL<>Fz;RY-wui9lQ6{?|$b?w>H;{MH^r;wY#~7ShJk#y?ywFZ#(~6ntI`HEf=gl ziGnK{*A2sqMpB00-BcBV)ne)!`HKoMQ<0clcfa<%AAaZ6ohO$j=2a~@{go$|4}?}M z9)*HR7sKmPo|94l?nPBH&HiHLM2+C*3SB|~tRtC$kOOtVlpX4BYN%^ysIPA?I(M4k zvKMO*K3cR{z#I;L!M0l&Kf+;Ay4DY(sXq6K55NCUKlqW4JpIhGXPO@7lJv2ykJ7k0ir%rMosI21D|Us? z;3%HUk~lyGb=tWR?57&5iT$f*`mC&4x&r6d>oGHDJ8 zo6mwz&jdhXC{5-E{lR=|YczXvQd8Fu1iMS0dU$v;`bM&lY!4Ox{Vcav?FhKv0x>vd zQy!i`7+8Vcc{Y;8E5-Ux!Kr>p*Cd;g0X9dUO13QRGt?WG8ku=e$ZFmQejfty>}UZ0(?#k&WAhiZ&W8K$@98plqd(A!Z*WTB?1xxVuGr$6@5Pdxq9CqDkMk3aSF^Hr7A z^`WHobSW=f9gPPdy3}#iMv=`kgGZMtPTn}W6a%=3p<2@Q^IBTm=Cz;>u^gQ-@8OJCi*2UtK^}C1bp$;#pvPRtarG<)_b(jJyW_WZS z6cPZ=JQ8;6E}hu#ZSI}i*f9^m^ttQ#r73s8(r=64?Gy$^X`asNb%EmGV{ zX~D}TczCR?*BY1rZWSqW>RFo9Bd_XwnONvuB>pTp$f7jMAV-c&)|!zXB-OTQ6eG!*`0sRyGNe*rE+i8C)XZ{IRbcqxDE*jx=E?GJ0+x81dj!XKBf?TFd`_Wni-)C4Qkj9S_NJ=y z&!0uEY-^~hJb%8%AZ=}9SFh`&2uHerOk!rVDifR9adFQ*|Mb(vGsUyJFPyKe5%cKl z4k6*X_@?$fwenQ|&ejv#Pwbz0^72nkj;tak)#|M{erF$sv(9lKQdy6q0ytkb;CSL& zOboO9#kHw{9wT7WlQu(^nW}k|;seAE$hDyjII-}DmZfRZrAO|BFzo^?Z6EmjGP|^f z#a7=$E&1AJ+S9UJdCBZ}_D5ZkN?P^Izsl;=da!761d^z-!ZStHC{{}VWLNs;Sv!rk zwP;E$q(Y{m#=u)KVt{epQfe$3PBL`fjgBoK%+8vLXF6QA3+~`S^YWThsxtT3kzq&1 z&l|d+JEAnr?xUbJz7HV4Wv2y~!Fe%#P10;EAO>LfglO+Q1^C^td}9l}HkB29VQL; zq2)9#V=`F2wP{pr_skHSlXl6@0j*mp35d!CKcG}0Fk&JxXa5A?I;Mlm9V`uYw$z?~ z_T$f-tE@cx%rjh4TOYYM*wV(8OY6|S6h(0iBa@q#@9vrNAqrPpQ(Iek@!a#zeE5(5 z_`m+i2cM~K8kY2nJ~e2y|hteTLcSpT|=cFeTtIguc7kAb5)g9RY+IWhS#9i zT}9h;_dv9Epgk~9N`M)sk6MNluh^P`0tUYD2*l^CzVjvNVmP@>9fD-Tfc%pkvO#Y` z8d9trJN(5cb@OH1eYv{ZUf>Ouu zLPuk&82_7zkfR2Ad$n|X`VI5cS69(YudQq5+2E`_0a$zc0v(0bGg|8E={9)NhJIaF z?V&$=zD7szVpB&qI#3^MOn)RbGq`?f!N}O^nMYy~mvnec%9rS|MTfVpNl*z@r*BF0 zfnSUMp-l0*-DZco0W6zqE@%yVBXh{*TWoiBgcj#47z#w-l2@G6pD%1 z)KFVh`Rs>2_<;{Ub-uA{Le{l0H!*R0e9p}A>a6Q)CSvp2^*i@od+otX_b-iVB1~G3 zmWM+I!M;Ylpdf7j-Gi5gxqu0=ZXvir!)S@lL55Yjy84D9V+#)$&)6d1ab!%3wYBbI zZIeWb;6MvFr5Kv;@AkcePRnk&E%j@$JK<5FRBRJy->%roRx^CNa4fch)~C@&zT2$4 z*51y3vGtY=IhBBVIrUpW6f_M29?}bCZ2&%hM0=Uk4>(neO%etIW4r*LrHp-)FWr?B zvW~KE1;*AMWmQQ6)OkAXr|DvR6JV}xD-Djx@rdoV6I!fkbi%#k#n5L|Inn)+0|KX0 zluVCj+6XnroPSG0b>;cWx|WXiruuf6jqC_sn0FTDvK6aqPY4A%0XT)1hK?^^FRmd{!z*5<;wzvJtk2QpLfF%j@e4CRO^%9f`5A ziqTso5mFy>$UIU{bJe-Z+A0mK=2rK?9E)BVp~?to+RkQy8)D^zyCCzm@2X-;a*cVT zQ^WIT6cc`9A=3V^f`EMtxw^&1OMx|H1bDyNQ{ICES3SlAe0&2zdCuDZ(!A{O6hG%G zrf;ft8SIjsQ6Ll1;7Tk&c(J|7ijj4|J7CFuc2e_`#pvIZ?OEf`$LJeqZ=s@9d;Vgr zi6U8UDER5J=^a`U#W_w*40WrRluRbFYOBsY|J)0eb(SntqnRig;O@kWm3P#D{=)(! zI##z@{&j4gO%y-4e(%nmyLaxtdiU1NpoA(H>m`7cu=Y|%-Pxx<{Nblwth)H(^Uu84 z)KnTk6v4+q4N+L`A3$~B3w>mvyFvWcpiUq-G*mSs8xjKaj-L7O2mky-AN=$8f9T`S zR@T%ucgxf3sw;0Z)%vz^D+3ni*R#UNW(5f_&lg$QfGD0GpP$}Ww0FX`VeZA`4bGO^ zfh-9Sp{79|7`k=^&)Ab3xwliYdpB>_;cQ{w_UCUGD4q) zYhH&|zBDf_OG{n8H_F9HWa2o;>T)Hp30xh=B&+Wm33NJPaWej-fF#n4z;FgTo11A( zH)C+uq^RlWR5-`vmH!Exu&-k3W;WrKl{bui)K*n10}OKWcbVZsFo4{Xo6EwZkiFkG zduHh5%0b2c?xBUJ^xm&sefg8GzWUn3Pks8;m+#qfcXH**)#Dul2$zxACQDrnmCrtV zwyMG8?~4~&T8D@60n8Oj^bn(so@&`oe;+BtxZB9Y;mOYI*l2GH#iNe4=7#2my6W@K zeBz@|ec}@z`@}OZ2v2Rs!MlB>vA&_bXU3x9oWCs$`@8D{c?lX6&f8;FWbvB|#j-{Q zY&M{+ zqhQ20GRG*~24ii<-X7XL;tuXXE~JSd{XjfXJz5s%By53l0bx?Xl)wxkT(+`_V1YgG z8#f%xDp7$CuPjdMd@X0sCJ8=v0zQt#rmtm{rkO&S^uh5_vG-+}mt7Czs8B(%=c3!? z6GI!|hUDI?ilq`XIz&>~5-9z)x_*YJ`I|I%v@AHJ%q_sZQnH(~(3cwn2x zWzCR`w%4C~@j`V&?S&VgKU-hh)7#P^y|x2HVzF~<;$XIz+9wI$>uOCNT-u(Um;hp_ zQd$kcS^0DJsgHg5T0Tsn#=W#W$mF> z1vL6fCMN(n1ap)83U4AkWZ-bGV)vFJAjKijQ&%^4aMr}$wpD0!kFdFr61)u;JJ~W} z{H4q4LjFrEaQtzYyhUcIB}E%4_d^UfwJ}}H?0Ca+*a#~|1^D=?Wf`2FR+H8zGQjt! z3P>43^j_W2LJS`Y%B$Y*ZfV8FX{)(-p(c}8=CIs8W?{h!I6zlafb^}dfRHhu9am@I zh~S=lQ3_C$XMHCN9WyUpew#V}pTOALns7TtXShM{S%(3>Ih#p?%}hy~?rp8FuBmJ6 z!sltYSl`-Y2&l)Qwe?Tjyt>OVQvTrh$D9_AsHrhT&O!r+-%j&qsABkagKDFLs+8Kw zv(G>G>@!cBJE(0S3;?sNtvUbv(=S%mGQ@c z1AQ;A^->f7;e8e&lu=BZhqiu6Cg{ncF$^P?3I(PPg#j%qHIu7Ad{E{}hvwqNN)`#n zLS-3+RVOjyWF%qf_&sUK-F3)@96%9}nzLMif_b#u86a?SP7)>BuYvI_1}4}d0BGfg zJ7vWG@enU=)n^fmxdXs~+F?_spluMsyq@pyxO&km&P@&LWJvv|heBA$OpV$E*Jp&Z z6yo&{%Yj!jBVdoBs-dY%JxgsCXS7eTV-JG>Cb`6SDf7SSmoWamu9$h7DxudT0KT?1 zTtNFwapPt6^3J0;!HD7CMY|)`G%!3(WH5!Vjbip!uIO5rLO91b$v@mX>w`*`b(ArMrCJb(_i<(>AK^v1s?afilt$lW!=F&aI_mH z_v84p7@Ri<(#k?St{FdZC49;d)g#uZT?I3dI~C0{lqj3(XqWf;+Heuh2x+zD)k5t* zoD8fus5qPlh3|(V#~d<@vr;Us1DZlvPeCBu1Ml>Wwp7bcZhc19mn~Y@ND>K#8_%S3 zEgJx~+RS1tFV7jZXpL^g zna>gWNoGwPa_@xyKiZF4G0gYIRS|!ciVqza@Oc{?snpX_-PqpLs`McC-QG8L^V-_D4%r?aU)~r9#@SyyS5?>8 z-ao#$ckQJcQcVTc_PVQWEP(m=NOqP4oxK`7Lav=a0D?G z4Bl1|j|0$3ehuT2Bd-bkel<5Y)K*tDYcy(|B?+?fQ$|;q=^f(h{%HTpNj*$nu)*kZ z1o2OSnaTfH16X`josl`6q=@o=e^x$xjw%ihOCY_{>0<6KIteTt(ZqW2J)mRx@E^nc zv!Yj8X;gv35?Ppw1ek*VG6kS))|k=Bb^6I`T)*6WSGczm3p+&lp}~sb9louczo|5g zYXjIIp}Or#Ma5RpI?>fnuvk$rQB3NWK+hxwdKQwSB-F*!nQwgX&;HAQ{?Gr{|MUK* zYuZW?^y)B5*c@pp4HuE41$^Al*ui`D5d)K3^bhp*8~R36A7b020;OY^I%%|7pByC_ zz-YztiFJ-!Lgh44^^p?#9$w~W1DkkSqSTU@XbV`Zw(NbSU*9eb4Fjx$KHpCn6J1P>cTeLdR zW^IUs1@r2L^UR>5TI+>-L%AfG=GRpg5z0f`_&<|p9zFH35B$mh`^SIsq37y4hGA<{ z{VnxPtu>9Uh{L|O)YVwm*h;9X%;YVzu{_v3U2|SI#Uw;QXUum}#-X<1c}c>C?}9@U z_r125P4tgxoAcfmDiguKhDPe2SmRAQl5mp-_0(ZFl{fFy=_& zA`vb^MM^Uzf5In<{$&EBg33&K_wD>Vs6%1)lO+awW|5Pm8H%p!7KD*+r%7r4!&A*+KlGXGK zMA28Gp6BE6*})ErAtQZ*nqFDm4P>sJg)T31EXp1Ec zvg!=!NEp!Vp-aR3eRg5o-)3iyA(LS(h~9rSb##OJrOY0?350ETXS=5b!}~3 zduRJ#e|vLdb8}N2QIwi$n($pSn_8k-6^4`O#Q)REN!nz7mtHz|%5m~9{sYL9)_-W0 zDZ-15LHyQ`GLSyCeMZwV@F^KvPtm{H*Vo0{o4B=E&a|E8a*vB&46eb=$z!L(4T4R& zr;BAhM#XM6-7o=J9oLUd7KqUYTVJbK9c66H#L7Ks!e_|;K=rm&eL9B8IK3whD9axG z4JC|U5$(2oTPj>KBMIJ2@~Ae{t16a;m~%0`6`!42Fik6tl^U><6-dO;BK+1#+2YVp zF;Jv}0U4lh6k~TJdwDUB1;(ZqV&noyD5*10Mp%H_Qy9ZZo%k6!X~E9~J0Q$cUL!tR zefHxY{L?@E$TPK_rJTI%fXZZ=@tTuc15vhu>&^F%;vU49;* zrL_r_d%=7`V{Hq`1U0f{ays-I7!gk2Lu61gevdXSMrzlrIuDMMkth=IjSH4021gsQcf_x5d9Kfv=l?#p+8V9Cxof!mO zMindZEXE+cdFGHIkocH+^p$x*8~@rc#1=uB)w^E^HZ38!#z!H-JMMrYpUw%YJtY^SH&ba*ytm;%BLeB(>F1@ikY2Nd+O|B&0tbI zu;Wpp#t?GIA}I`A>qNqb!bxvew(h$`o}}8EvgtH>ULhL~6}^wtCwv$TfzJ;pD1-4J z`+JAsJ-nBnkW@aU27EV#K2zWgGM*81A(o(Y<8^#UDS>a=L0URF*hE3^C8;6*e!PNZ zB2#(uYWamldsw(+)%e(>Ded-X=Ss|{(#tYshor1nL2eJ8hp7}mOKBokNf+9?)@2nu{fg&Q zCF}YupP8+G;UgdTlRy5z$1k>(MoR5P)BW~9<$7{yoJ`GV?z{2QhuHqaSQM)sXOB_? zY&KLT@z~F;n0+l`CHN}AM?huP!-gak!3d8H1yCkD)?eyxuWf2?t-nxtp}Mw4H@m*k zL_pE92!U-inF7`wFLjr|#{l+gsksw>(``k!5(07~y<93hENs=CI4gaQuztp5<1x4k zd}tPK4nQ3m)uEF4g`#st#A(y=0h9pCi@`^alc0fqtU;h3dTy@27mEmLCbeP^dHHY* zQpYhX5j3Dv>Oh8oGD0i<(%u?boFO?H`em7uJeEv6=DDc36{T&NFSd>J-Y+=CBmstL zBsO<>hI-_O^<;0weJF7k5XY7Vg<$gsE;oG6;lr+acJ$}zGPg?U1lPN z2|8&kF%ql`$)Pr) zM7)f-%`q3fpd$h}vwwZISa?Lo3jd~D+)gDU^)dYXpc>^z<8o{X+dinbinYc(5|I~{ zwz2!99@8UzczvzJ1+=Zo!8k=&4MzZ%Y;No5vOcI;xANI%pL*(igSv8U>(0G9cZngw z!`4n9E?Xa*6>Eh#qflWCRx*%+o#U8tCXC(K(*hGfV)fe$Gy! zpf7S*Z^7cr#9y^4LClQA@N6+Dxp5LBV@|t%6C3bf%QS5m9S62=;6*@Kn8aW|6>ZDVvg@OWqv5Z6-UPiU%*_HJ&a@cWK3`_|n;N$f=g-@UXtv0EHB}68B(*vmmWz z6WmuQaDjAFlWY7`Sc~}EM1U9uClt)DYymJo!(rOA3!tTe=E|o({GpG%Pz?-;Fy6?R zXV^3177ll{DxP}Nu?#_ZNDd=oMH-9+g#TiY2_h`M%}wwfh94EM-ZOA!I6_>*Eaz_o z49NpXGI^&n_7uaQ7VXDFCk~kmCz++A(_}I%q-0jap3-M7Lx|%(YBo))tIFdHAu#x% zN|UuGHqKlz&xIU(vA)?1&JY_;b7*YL*$29VM1n-qRnv0$fh<)>%B#;B;p4>2_Xt5Q zdQdl>a*LjW?AC9&&sAM-KMVPjQOKl;F zQ(se4d7+_mV03Qx@Rj>7y?m_A0v1qkZIsq4RzK&Ciim%x&e?t0>UM0hLD4K{17#Sf z3>KvR5?9k@v7t`1?gR7=6y1wG!@)Z9^ZN9bLJ=f7R_>%~jM2stWY)E-f3_vtn zFIN5;upfJO6*f2bbwMyMyBaS%_v{Om^*xv;X7@*RIXZhY9yHVeX>9A(x^NnbQLRHH zCDV;Lf4jfEW=@aOq!btrz{kirgSl&2NLNORu^-5;9c7Yh36HPFcLybqri;Dd!sPGp zUO}An{>xpEgdlzZcEAhB>JUTdvFzJk9bJl;wDW&5m;WU*$E{5b9mx7a#9+F4M^m1C z!WpxjGHYU;?Pz9nF+HewnfjF^d3oLYGqq8EWImG%l#!YA%C^YF zPfxpeob1e)0nqNQ{L??!rGH&ln>?Vt(mJRnoR;bro_+D$i{$ZNh~k8jaCGlj5Y~EU z`enkTd%a$<_T>6s$4%$LXE@O3mT9ghY(GY2k^1<;xXd7t(^H|93WTEo2vSr%_{Bzo z=~s33l(<;$tK3fDDT=irNleAM;%;@MJpA;e-Mo(dfb)0heK1(ZL|5avr$749_y6hp zKls6qR@OH+>A#tdd`Fvl5HdEGnS?$gT51B&ml=(qn7OJ_7UyO2ugDpH9cL2J;1sc- z4fpH%PTaZY!$|p1T$l!aSalHoV$aE^-QW1 z*E2#8tcnA({=?wuma!uVt?`ILWu<{M&!K58VsDgYnPPY*dC17ZQd)5rWVX%NWib*%$w0M7M02QZ` zzi(bDF9i!jF_*sf{EHW=E*9r@C}mR}XziFhz7a~I!9s)_FP|%kkyC7s`a!sP#om}7 zWcvxh<+0paY1@9EC# z@ylhl58{jY@GFqrtUhF83?RA4@^DAhb01>>ANa^eKE?p(>S?Ma7SQv5g8R8`kik`%aj?(Dhq zl=1D4?VG=L=kk$VJSIv^CgI~GzpsF~jpAZB>SA|{3|E-h75>QA_Z|JH3ZRS|PBC$* z5LpDM$LJ7Ak&1ykZaz_(lKexGmI+&-Q{oa7U)-<<-?%VKQI0O*FI@W(F-Y zDM=2VVFXAlUw(}E`2S>>A@O3jK0bJyd*02!$LCWPm2?^13|s=D&E0Ik4H+BO%k6Ax zXsYYbmr>KeGoWdBRca{(j(Qo*H&9El9BlS;a>_DqdOau?rd+)B{>jPebl95XQ(IB6 z8TQ(P8C^@32Y+v*CDYhDF>XY#sl90aMoY7W2u}X|#k$T)KA@q!HQGUxg0{8QcCTV0 zX-K*KC>e&xwgVCK31w=!L@%8e2S*pj6%uud)roAe<|@ZWiqY528%H5R!aP8*GEhz6 zVC3L zUvFt?XlgE6?{##AOM|t!ydqxvtMdd?C#ftiZmyDvUf_ke**wG00F47ITEGxOz$UD5 z9dtk_Cn#V-md;8A?ah}}a;B@wbRu71MjVLgbY~DqCm0(Bnd|S00>=V+jhVKdkG~7> z7q~RLeTuzee+0YGdWdx?-;i%BMoZJh{1!A~oP4YZV2>;tqZruN-qPA=7`vE(^mvfg z-)0ao6>ONmnU|Fr>65O#?WG@M<$}v0H3j+Qq4ilCVm@eaZf|*GXMGghA`PL%foSKY zk<^>p24`mmCI68K2F_^5S5;#vcfkcKo4dP*02~X;+Xslms&sI!7;!h{Y)99SzP5_7xno{M?ji|sgeHfZYdL|GBBtWCdFIY-OG^J z*7k~Oznsz^i-ZTn`c19iQUrzZ-ki{iPm>9t-D5ouivPojRqqEej;D$rck5(ESgN9uEHxoQOVrteY#e>D@#N-Uj(<3Fr^QZb`+Zwyj?>K@ z8rEml3f6846k=a}YWpDr1(bm6yN1RqW`3C#3dDUmJ1t?DFl#wps<8ZEg(Q)>%tV>i z-9nlPV`lg8e}rGuk9Ep0n>a)9q5yTgBul>{}wA!ce5FW1F=q6nfES4kUnS2&uQI<%5XYE}jS^2|c4#01W_j#ydOo~X!#qP$+ z>@-xqt>Gfi|J=FC3opKKw#f@k&KWNVn$X?rSHHwt`M$Khot@j@KjX;ZYi;Zs^xRrg zJ-fdir6bn&pPX8zAwD5t*|P?0Z>?Au_UCiO>@X674&#>ORh^T(5BbBn%J1aaT7FTNn=-6oj?C#RZU$>FF9-a^d3OQ zA-6!h!gH!SiH7Ns=Xuv;9?iwmyofMHqogaEB>!s|8b)XaXKV8gf|B;H#=w+ zZFDE0&7U)mNbZ%@)*fegkU%qDp8;=kbi`-OZE43M2%2^4Y7RyHuV9p;>c_PXb@;|BTI7{f5p~6BD8vahxH2NM4=z@84QA7KPM1uxjm%?WR5|-s=c+S8`Z6r z%$ObEIf(M+W~j?HiCjBM4o>L92t4NyiXx-GeD&I${iX4-V&bQY5iepwiDeBT;gu*v zE|wsimOrAC$u=yo@xLx)@@iXTDy>H9vS^(kXardN%;g;v>GTG%q)p;nIXSv=LX!_k z6J2}%&{~*^BbyyAs}?e*B{XUTvhC=~S9oHQZ!T5o#qCZ{S4@qRDGSBid!pN%5Q@F& zSqc6s84e^?7T)`c&t2ZLdej)=%0*Fr%kfQonenJteH6H=AAt2K9I^NJIf8vV)YIJBW@UE zLP?jdmm*IELJ$nPVm|xmvKS!B@Kx{wc}oSvReq>SR1pRp0b&#f(*}%b4jJcXPRShjk=HDxN-+z;jB3nKsp$KwezDTj*w8HePe0!EZ!pzbUtfz4 zk|_mx)EBE7v0H{Fm=lCWWB>8#A3FD_1oA6HMvjlKU&T{BykuaVq{gn8fIN?!e0W&i zXtS>7as!+{+=@dCV_dkEUnSj}Ko#7~7gA7%q#1zHT~7_Wy}w!PdreIEFg4~fNb#_0oPa7&Cwqnt6I_` zp`FG8OlAf7n?@v5aN1pxITC}m8S5#I#v(f6x;}AWJv7azWywM7b*e{Lv|r*z(yp`EI?t2neZP& ziC;xx!^4HZlzm`0OeI`0k>6y3{Qs6Vko*}4nH&&7$Cxg4DZ)2Y>jW#IqcoD#mY|iP z!ix2`QrOSWDqFfsqf%Vj!XC=P6ezc^Hy6fiD9y;85I<-?q#9b9PRCex%!zw8NeYy? z+>$Z?n?VC0R7p+M=!VQF3yCYPEMeMDEulGwAjyPF#n6t1qKzx)kHh1bY$oxshr8NM z+*Z|4O>KBo-=WCBa5rSy6TZ1l?UcOVZ$&*^bc{QNX;5{zkRb(Eocd#1XDQtCDSa9f zzzBt9blJ}rs+o>{un5YB>a@=DbOI0KjOG_3AGcsBXzg;dm z<%Eg=a#9_h8r3M-q1h~qP}p3b&pqbj{D%c_1EpM|3Utp{ z*`V418mSzfyuxHIk+zgvNi*sP;8aNE`VxythY-agLANakDNaiG!&&ZOzZ)3H~pm0W9_2;D$f2` zG4epiSyDP1txkm-kDhlT%JPeSqu?GjoGkznHBuRJ-lHvM?MD`)TeugQ*YQC@1P8}D zBHUy6$5C-mIx1GhF>R`}o)Nc$k}gHupgd>0wkJK_%Mi19uKoS@?VTB%n5dZE=OUpX z5{hzV8O7N$WGpdYbhh*reZwJ(6yx`G982QGL{GOBA%yC6N@k7nazxg>mkD#5D2!z$ zQkX3;CjZ;0SQ}qA_NdYwubBAcSn5CtKq_(j!YRbxUf^(c#kNo}u3?NI;3J&B1ftSt!b zX;hWdXjp7Y{s@kUfrycB0m~hJ*Z_u`mxEQ-ISwP2#Y-p=JHnSEmgu3By=S%YjQ>KI&z#m2{t_T^4H#@%9J5&CEsZvj z1Iai(Pc0Tsu0KM+RGX;+n_EmMvV^S4G?JlETfcN-j&OmZ;MMy_fT&zMld!NAO881L z-!e5|++Yu-0C_{k=%4auHM>nRA4k)dzC0$8@HjuTNkK2&Ta1*X1s;K-h z)z-@9ww_M(T-0*u@e^dw>Qxg`?JeeZ4L$nivEja!HcJo`25qe^X4;HCf`>YazDI+s zK{#vgBvMBa5o}iK@8CZ|MF6c#kM(r*((zJyrO68wZEvZksXG$lT3Hy}#wK-}{qJKm zZiIX_U3ri-I2Jo#*QumC+LY2wiVxsWI3?*_y7tA!zDv1C+#_rWE?)v*+JvKtefb>Q zTZTP1&m2c#uiPvacKnpY3>Ks`Wffk_>R0JOF}$IJY!*bT%;Xe?PIngp*RV(ji@v{3 zheIYTyG@FcNi6RupQA&GM!k_(2kLjZ{R#Xlj|qOQ#`fql z_Ty=}lwJ<1YDy~)C8%M#6<`{eAJp7qW(TstJSvhDdRVLk7#M&vvQ^f*a>??oV#Rt} zx6#KK?6A`>L?WJmb}s;R%)DM9-<5n69pCC5K=&iUth)qe#f(=>Qll7JobfnI8Fb|r zjBS-rwOh<9N8CcoKudUs#Pg*~6armnH=11vev0(z%}}!<)hsJOrfq6!Gk86=s9(KW zv3OY-5OhYC?ysoA{N2asFMR>fDTtQn*}jRkGuE?YQijyT+-4SgD*%^y4Z0x03_rMt z>oGD|lpb-}b~%{*a84GA^rcx;##2?-oDx>~nNVxq{oOmfGHT{$VI_ zw~;p-92nEwlzsVqN+nzqP5^e+s~3R=Vy4NIcov#=n9CS_G`@ZHy7ew##Qkdzsk&=R z#dp&lk?rEji0-*10ls8{uoXCM{?N0f21sL=4kT~Yja4;3V==RxbejUL#0U)a(0DEb zCxCmG6}}n>ZDV)eIM=G3d~Ur&0J1D`ka%d=mPsJ1v$7`srqph$Djz14$&09L#0zw> zI_n*>{H(|RpBa^SvGg%p{jPX?}lqTXrrR3{s3QuWuh*xpoP~ zY==t+`uKD~0mB0KlLG^|ZAnS{I@+ZlW7AOyUtKH~)`R&o0ttYR6$A-Mi*na~aWoT! z-N=mW1`2xL?wKp3(r`O;C%m}O;&!EvtXWx3Gy)0`U!NEmC`xaHN2E6)hu0w*mAA&z zjyk-4l-`C`xQHDvz6Y}5g<@FlA9ocrOFAvH4Af#a5wYfF(aSnieSiEU7ssTiJ zVHPB7y5q!kp$*xvobgOOPv$gC+c4Uda}&L-&GmXDgENoC#Uc(B94k4fl;kr4L66n1 zo|9LrX{fF5D7Q^^4s=T3U`%!$04w1wb0s517J5+z@n$56n@?;A1eYUbN4t%luG~^k zfbh#qWN}grZ3R!(Y|`9RF?#TLxV+q@$}EpuMtvebOG0N8L>E>afmVgp9DYtpsK-*X z0kh>Io_E$-VvGpX#rWq<$C=QS#mZ^0B2$6}h?tGgb+9t27V~Bi)l1?TsOgdl!3Zx{NwKav10lQ5*hh4X)4k*(gfe%FOlS>n1PfMJ|>=` z4qzlL%?@?8;mh?-u7bQ3IcXj60>$hXm603=kBUeIo@8z|cQhiKcuHq3O80yfoc_cv1)$4{CA1M8D5a0z2Reo>2d7q12&|w7UT3Tr`~o@u zkV@mdC50H_DNkbpko9QMC`|P}y)PI?Oi8Wfr>QMH%Ro~8v{)kXX$uxd@ZIo@jiAK@ z!g=9l(1Uic^i7-qX+aXk$4b4Wsd?(9vVp^LgAvalu?c<)I_$)&7#thx#CC)a_l*$D zll}~4kSbp#l$u?+W|WHFcNCD06K8B(at`^Efvnmq+S=9CKTV`2TcXv|;&i!YF;bmb zF3a~?NLztZ>6*0m%7A7TK;$r{gS~)tp*Ac3eYQPNs?k1iAwcd1NSF%L?wPL!!%uc? zmx!!*!JU_|a^TYLsN_iq8D(0O=?Q_=po}wfs7zR=%3%S7zXG#qxeork%&3gisC+*h z#q!NAhq)ZDc>q&Na(U0rc(U#Qze%~kXh{Gh#5QrF%zsM8tJ=^yjnE;AwMCz4=STx4D>pa;ReZ&|{X;T1ZjhbHjCQuuEuk;S zLBvx^vxduyxW@4{z!>c0~kzYI)a^^f|&H*o#6;7_xi{TRC~;@J~f;bU4$&mg+l5F3S~ z=WijCNj`S5Dg&2#M2T0ANFC1$>L3z=+B!3m8zaa32F_CQB%T5(A)yvwTpEWLD^D!r zCyF{Knc~T#2`eJp<8lfOR&kR4){d)Us=YwSM1L30gJC) zIy`_IYZSxU$-82Bz^tun=JZL@?i?Kw`2EuU<%k=S;MoK=b{azKo7VgHDK*h!0QO`KptRfy5fnsvNin z;GR-8zMAFz^(fxUPUm-;z$@P^c0Bpbzi3XaK{n@>x-r@Nf zny?u>=L;mIY5KaMP|w_76x(jq`2_lKfX6|PQVbI^JwzAJ)0A*}E4um_dR>$y>UM8S z2ro0Ka`q+&$Rby5wS8;=;KgMfUPXp7VU7CIUi7XMO-UM8M0MvM$x45iLBpx>Vt8KN z5bX`Ao>D?P*r!Sm|RFwC{3B+AbZn_0-d}J z01cSqaXoec_;U#F-HeY9$e8wJf`|a+Vj#!AN4fQI|1igoml;-G z5Hqarc}0gwt+9tpn|9-Xp#X!M(%Eh?2XerQRZ(h@B{O17+d!>Sp(Vl`{AFirThaA( zo7ZY#pq+h`T&Uu~n?Wv2qph_Q9^W&B4GKci5lZgzm>mRETA1M?r)EYqDv&G*ag4#2 zOM^HbB?AO|B)CS$=z;`C%=d-+6s}k#Db%&^+c|ShG@VkB89X%V?>c6+3mgMjGs+9~ z1W+mO8J?xV`L{Yk0g&`&DVVxWt6Lg^X|GI^UrmBFW7(+$u0Ne)g^%f9wYefv$~blT zeLQ{@I`c>~>Km`~q)mjrzsTli=H>#48tZSyFaI#dAO@eNDBIbH>FKAaeRyfq+$T34h2<`_gLy>9E#O8zZx}LM;25Ho2NET$x%J&^FF(9<{RFJ) zZt8l*D%qQCWMpt;di(B!d$%@M_OIN&$>campOV*I<~aSxyl{%gDbCp1cYlunMuunN z*z&aPTG4Tuxx(&q`=jHP%>@}#9O)M)Xw#WX=#e>6RMSo;rEm&2v74I_O!W59?M1^+JNzl=I@mlku0J7;!5mT=WGEE&*5Q$^<~?CW4sl9-t$n z+jO9V=^;SRvBl*1vsrfw9SZw3*lYX*Gf04+yYcGXJw;-OxA^Wfs{u*hvOo+xEL3VF zfG0EZDmJ8{lmrq30)nvx!V9Zycjx5h-IrfFar3Ft+PfPEKvSOs(D2%ixHi(`{v+f{ zd$(ZeU>vj#g^46oJaSeRk?3vk&fEgJZK(Ux}!Q0SdMkU$^)2>ksbj zZk*hF<(?YQFJ%QRaf>Rf)+&C2iI=(QdF0AaHGus7x==P4&60QI@qkz`iw7VV7PCD# zXjwp$dEIE8ieQ03JVxl`aq>Y$g@fK+=2$NHU`t30@a<{L1rVo}Z3gl<_RRIAGb|kI zvq0c9h>NK^%#S@_oKThky87i5b?N9JrJt4mLxf4|PC(Qtd~*MNhi9BB32tv*H$i!k zapOk=U7BjbZB}u(oB0URdZw0X*Tnm!bSr3GlG5G|2gQ$FhV#m=%Lbq(PPU<dv1d%dac31AJ)yZ4NuaWBp){Lz0IS$zE$k z7505c0+|33iOd8NI}$qzwRUxPvAfwMMN-meXpKY5NMntx(2Nyfg+i9cBQKh9RQPjV zF&vI2d#OSJFW>im?{e&^Y{vmgHG^|R~r ze3xv=i1c3V4Nx}Nh|6+lzuU`$ozo|uzGCY*Vi|MTkp~h+_06dkP`0nskn!?y>~FGt z?LB?3m+nhi_&i`P5}zrBCc>hfSq1(}5*7i;FaX8lv>n}MmM2wLlf~F6uuNr;lA51_ zzgmFeGV_<=5zED#c+vblfT34nwj^U34kZ3POwt;Oe}eT5F)gN_|4KMfWb-E`N{peue=sXmMkb<|Sk`_ECYDVXFar|! zs)yRv!M^vk%JZx$5aM+=v)Sm~4s@8Aq<|g4H+IfnefH}4Ap>wB9I%kj98n75ncah6=b1h7nKB$8TdDtfT7D8B zx)<^Zsf2l3QFW4d5-HvL{rB%ahQ2%;8=qT+zaapvyVpmsrXbX+TnLAeTf_CRr^=eH zM*3es3-2^S4jisG?!MuG`$A8ybki#|=w}kqn=#@2%a~YY$&nMQl@KO1oSw-2w#Ox< zt@W)CG=X6WJslK4yH6mddOw$uPr!mU6DygrQp5bkzjjBXE2Rded80G#z4NigEaVC< z50h3IM{7rcons7%z|U5-@jcNLWaB{@*tM=eN?_^z-tOVK<{%7j)BBlU?8_fCCRMCx z_O0N9kBy|qHKL4B-7Xg!KXhO`wN9`t9H2Ds-N09C*9I?hpPv$Ccq?x=;smk+9 z&sG*zGbI;^N&*nvGf~P7>2o@GZN0ImaTJv^y%#Yq`c5trE?(6PczN8(5F+`ZK6-EI zYq+aIa-r%WB;Bry=)-@7kfnE#ct&AZhR2X#9(q4q!lqyLNY?g}TBgFV<_~(&`~zn6 zs<^bJ_=BY+E@m!_xa+shdKk9t3R$J(_)`7b_rsS+S|LotzJ@0f1tRmOVH!e$K}XeO zE}oa~Z;kEala$%T@=kR_Vl~8uHhwBH6l+XNW~bx@6o2Tounu<$_(~MVC6~K_*s*D*g>Icz!}T@b4Jl} zs?zN&?s7#Sy&g0%!DlF{r>hkHcKup;)1o1v!`)bK?C%5^Vsvg^vC6-SU`ag9Bm$Yh zP^ny^)WFD*9_!8b3`%^d<8G$Z!&Q|1~#5=qFV<`r%G@dq?P zkk1~E^o{;DD#Fjg7c|7}Ak+U_VeV&x}4-CMkwK zk|{0jt9EqqrWQB$G_J!WXdO4JbJGheW={L2&eei)GMG6h9TECvE;)eOrnfdXJJ{cE zN7-yKv(C}02XsO+18k%2wOLgz-78DBocfKsao92lsem#g{*&H`@gt)yU$ECgHYf%$K4tWZdpm}HIaI&UGkRc4FH zZxnO1NuW{0Gp4C+)1MFlnmssFE$U8J1LsvivRz!=VO?GB^(Cya9qD~ z|24N%vYkC^3?QFmb(I6Uw!oa#g^7{L4En{>GBG6736eu9YB<|OG5!{zVfwJOI&Wro zrM{zJXs)32j4|6E-RZnD41Wi$Fyu0MY(6^y5FNUppzmmaYbuq!k@9i5RB&A~Ha z{(DG;3Ygw?f&f|!8*8@3nUfRNgJs39RhMVR`^NrMmwPT0XpJi$#<_r#B+NAy)r5l? zlvua*y)*!*fXkk)%j)W;iY3kc;-5CEx18;PE|IkC=;xL%yqZdm#{uLX9qf`%^zf72{-K~!l+sraRF|S zTw{?u+K5%PtObcI_dg^Vf2x>YqaO5*!<_hDtp8DL8Pzj`A=D&Gj+392oB^kYuDt1wp`QhFczz6PU71Iiu|dd`Rjr9cmqf^Gj(+CkqdRwuqKxS_;dlq~ z($YlVTC$FISJpmLQGiI=xgdxf#>6xx6j=pPyRckQWKa^^eoFF|^F>*MzfKqmt!qY zP{JUKMeZ!2ah1^$Ek0$QGM!_Odv*GXz?6-?Z*{%cnrj#*yNsxy1C1Br4P^pqm=q=c zSbDJXLZIy2T<=0}wddbUDl@*Cabll1^>6}CVm=b9;zCjYuFE6^uFQ6e@mqGkzjM4L z94-<@F!W$RPJLZ=?Ho1MgR~w8vex0KC2LvayJhj3W-C8M?r$D=(zo?{{=N;-4H?H5 zgrVjI3n$G0N$*S`1Np$;v-6v0w@kopsy!==fIcPOdvE<&mUGBQ!` zY(T2VozkBaz*0EtXYu!~V)LrBLu?919tZ%OgK&f`VyaNemUkVB&C*iuO7CV*1-OLF zvtBK-qST|Bo_Ni$sU0<9NN*kEtO{gNoGAy(pTX`?HZ38AE2aC>~~EX zJbU{5+49djoWAqY?$PCuFBjMBYiMW-SUzBmZ>JJku=}#OQdECR z=1zsNb^C>V3Jz5hEG7HIqVA~)FD0oGG-`?Y!A=B|E%7;iU)>|kUhb=%sUzW@{#xRF zrUS6(EYIj1a1GgfkzH6trLOoA`Vp0gEXuePHNWfqtS&5b)aYgqLAK>hGA+gi#Rz36 z8B31WC~x zL=g~H=P^yXq6`Yt?Zmrl>Kcg|+%gc;?99mLzVMK&s z9UG1~>t2cj-P5y+;_@vbl}{|^MZgx{OnHwRd^~8PGz|DiON_)o!dw zsMhLjs?QYdMP?OaLUFNm!wI;VE_}iOqEn`>T$apo*|N5@mR5sW)qN|fZSrAU2MiJ4 zGi+e?0|qYC`2=^?EDK-9Y}#~6$#Lsz??JGY2ht4&Cy9Dq-k?Z0sLpfoqC-6}=^*XZEL@Zll%j?_Q zr?=;ZxirC&3@=X2RjK#$VeT^DI`fIV8c4*IG8fUP@s6Mv~cG(}`W3a2!q zL^qB+P*AD@9O>4N@*E{r9DcOSn_qB-$roKU(Mlo(TM@aU=7B#IwU#Vw_I0(Y{lL|p zUz9ID-_QLkXD1kkPgVNk0H6$od)J((@i%TDphx_AdVRK$yA7my@TAih&qwC7JFaspIuPB-D%Y+j%&VFkm2i_&Pu=8nzO@mN}+l_tjEzsPo+E$NU{Qf#M%vn zONa`4&g~Z@0XV#>7p!FuZbv z1(S#)X^Q8MkXdpNA3guBO5o3Z-#84NfL+jcx-`D{SI7SXfEqCc-%jP39hYbUAs^X4 zoJ7{fNvS}p3+a1OSnIq)OpWxtOagZ7H465gOo&7zIxQ?9L3 z864{cIf}E(4AG>O12gf}!RORbwaBr$?MPRpKD03eJvX>}Xh3Pthjl`w?{KH8Oebl!k8$Q0TF zYau9-gv&Wd-PZqx3HlIfM5HvopFEF$SZnXLwh=WE5Cx0cYVOvoZe`BPT@tg+kzjHF z)^dP?yPQ0<4%d_C3X~n_%kQ+-!ua$p`Zk_L#fMn%8j1MelVX5e5@RDIY6;kuNq@VR z!l=Z9s#_1e-cuaDJ+cu{TU~zWlVMqj1EPRbBQgUBG9ZjR6XF!iE-sIs;;91Aq@;-m zj06jcxDJw&&4Ej_2WZY9tUV_D{=MH|0;YfO;Xuz#kC7uSQa>0SGi{l#if?fvugH@MAm7E{e0arzgiZPp^gD3|hI3ULT#tMm9ft z4|?e<^UN;XDyS$UYrbGpD)z$0HZXT$9B!1Sys7! z(AjLgr)Efw#rU8-`ZLtdn#T3E+A@w%!65d6r~B#2>}$_Pn)O*01~ zuuhPnsN(#s)3>MY;^O@D=<4Rel{>AI-t)BGg3VP=yJZCgEd)Y(J5au->y{f3@98`et9 zNiCYV(nyQMO$(Ft-Q-7htz#!h+K$=2X|j)Hre9bvH2PA-FKlL6(RwL~rB` z!hES99^(324xpGy7{KMnDkD!$R2;ae!UL`-${l@5ktcAm?bv{M5Ds0VALK_W11*v%p;v~0*fm?OKDG^WAvua+B^H@oZP z%SY!{8Y_l3fuS`2${kZ++-a?oNkeMs^sz>QNW0m8nn;_@r^Xteli=AlypB!0hc3X? zG4-%Pn1ViW2s_ej&Lm;qOc2f@2(xUTDA^=|tjPqfk1VjWp7eZ>O{cQcWC#;@xZfmA zlpJlhqgL_E>hQ-SGeMEO&&7%*EnH3umKiKND2O`9>x$T%8^T8^`K^%)uEPstQRq(3 z(%JbLp_=00UxJP$<9kFNuIxkxJ_ZEb@~TV44rvXzjvTz0l#EN#k|HoF0GfZWP1nW* z#wMPikVlKBFfo6K;W*fzRbuZG1hEvxAg~o9Lo_$Q1>gs`aihk5YZl2{?kf^N<xtRePX3OV5%@xarm z0mA0P%P{VQIy)PnFOJ^9Wl$V_ta1J9u(3EdM(4Ac2DpBmv02@=kd@4g&dw~Eisqp7 zp$TN1944shhzWf{cKSYlkTAKi-95dGlu4-`p9>U7mv7@zmdgax z{$9u{7W_)DFt0H&$2DKI09il~a15nuAdvl5NlZY?l5#uxtIrsDQY?)&k zYZNGtkdhCb*tjJX%5KEVE20HgF`7VRCDI8?)77^;vmoII1?V zB4Bmc36dzVyDEU$S`Q~%5ttJp8ay+zux-*(m;$l3`?fE3(c{)D zfTvL{E`bUZWA0J5yKMNBZy1>{t4U_)QvusRP%L0y7BC^xlc%VEFDl63rPx{Wo_5KiXrJ^GOL9T$44g?3~=(UhE`q(5M3&+bA58#6ts1 zRRS<*_8?P0p!lr8B8Tmm>)Y*|9d!@3#B(>sy;yIazxeW-&!3&{yHg%e3Xx4=1?}Aw zSa9D+6qL1tR@{1a@yoJUFDfk z0QcDJ%*Zfi>k7u9EfF&s&r{PhiKd1>`QESpH@|gf#B~2g)oi;24DO^8X;8U_#t8wWgTQ6B zg8SmC*?{H(5U^PJ*YW}vm)&Um)N0FeYZ!_d*P($Ka}@GyV|#0J6N3ZeBub>|h+K2v zPWB=R!5_69cXH(3g9&NlR%AzYS3Q6Wl>IAikbFSr9Po{BvR~M84joh_L`tf&Zr#mN z)ku*SuBEMLDc1g)mI^;nF`OT%9KJ^{T;LP{G^5pQ88u$0?p+`uo`k0wb?HpQ%OJh} zvRj;;TC33>kd8;b7ta!OaeQtH=j^DJ*k0J;z)4)b{OTuPzPb)uF=$({xhlOC+C7r; zAEyNYyf&Mh}YdEt7(gB$%I= zpjJz@d<@5W{qxoEx#|CaJl4GIaf`XW_h4jUMdY%7(!{b$n?ljeY&q8k+>^p>??mrp zPjULM%C-i$Jxt4kv)kL}m&S3NDx?{>A@)ORVN{-4f&$0Sad3c*4FVQ2;ly4BD-9@n zO8QhGDabvpLz_~cPn&hfY^@vhVBD=JF zyMKR-c-STupVsl#+szP?0qt?3sZKZ`&qgfaL?EMyq)CYNbGlof_98Z#$TBGtL)`{J ztOC?smGLRF6i5}AEHQz)JjAt>@yAzKbAIvi)zc%87uW(*M_Q|*936b5&2Sphpj?%+ zRdI<*5&~0rO4PGM6BVT&crW8j+m|oC`th5WSNw1~A{_gTKCgVi=r}vcyrXBgj}g7I zwz)&l>$KYih2b=#KkWV;uiTaa>L(}p3ERwYntX70csi+#%p1fZk6E6Xu^@ec1%x(G zuga#-5_JAnLRy|5y#L|%?~lxv7SOEQQsDSNRUmRIMO<%V2P z%0jgJY_;U(bFmWxJVU$K`ehX2`2$;-L6wBc!Bc7=N5H3vl5lHs1=8Q#ph&VY31taJ zh4j~2a}_opMQ2OSXXnjZaBUUxHaspQKz%r3;zMrdD^U%A!51@(@Sf-&7py} zu0>>VEQo>~*s|umiN;Mr3&#iYSR&HIm-V7+*p5p}y0P+OICy0_B4XLeAtjXk6T$fV zS?n~k9HWCq9g#u=gF0!y4Y`agJxd1Ui8vtk?^uqPg$nrl3Jho%4$|1*&;;7GfU&q{ zJ<`cht8?>^ za!?umgYSIzH~-sT`!DT-`tJJ{^xPlWrB3m1NWwk!@H@Zx&%XE3#~-}^{)Z%b9}W)) z1w%bW_hmd9d?14shS0e$=gtC=a+6*t7`29#6`7sh7PmTkg=9QKyMDtQTyi4Pdu*Ua z`*>e!P@{S+SqsXfdeBOB)!aNe|MFYi3UX8A#aVyv+&?)bO4P+`>O&NakGP77n z1m4=ozV=h9p(p$7bcdoWk|)VuMRKFH-#tA)F+gbX`*Egzw&ib*8~_L&0U57UVxy*U zHUHOYFB7c5+7KX=fzfGw8Mz=9`GHm|W{6J$oMJPyZWMuY^OMEIPqVBC7J}`Srg_E! z1Lq-D5&rhWw|7r&uc^ZVGuVNNf>*EJ{P>$6-l*L9)x3nF{o_#PlBPOW=#NOilLY#=RR!{&@W*vOZ@Ko;@see9~mgN+iz-#|I*FN zHJh0jumXFE$mN}n2(bUgfBj$lXaD5ae&hR}*cd&6csMvPCLvfHzw_aTAK$xQe7sEV z>e1l%Xzyt6cu(cLYs|{w7q-`XyGG;K?@l=%5vr`24Re`qC9p2GQ`f2OKo){1`)k{t zq~KqHFug=YvX&QjkbYVOY+MaCphYX(Wm2&T)5=Zsd4H>O3- zb^GG^CFLB%jyr4rR2_y60uHiUx79kzL($$nI)rC8_fEPOUASTPBe-_0j!I6I!Nria zl9gm{4-)shbfc*KA0k2at3oYwvc^ouz_^iAo7SAL=S(8CliD&QDHg@^XhYyq!Y68I zuRf!d5lmOF538{SO1bFLgL`smnpPq2l+ov2zI^lJAHBIg${Om39qFGNyyGOwbQ-v! ziv)SC*&n`ors-!@dPdU7J9SZ>0A)#l)es_qoC~(xH@nG`Ev7VIoE!S+lSd;XcB3uD z#YqKfKQt;d@u(lBH9PwF{@st?hspkv|NOuDwO{|<$M!;uS|V;38;QI+^YHGS`;P`5 z+`s>@pIXP{XzxhxSkIeKt{tn2pB)>~rUQOTIY;#1uXr{kE4W)>!-kbPvE;~B+Spg@ z{7pEVEFiEcKr9{s1z{cjA@>0Is_p_Oz)q3{2kbAiVetISl0F;0m4`3i=gN96%I4OJ z7{F&M$$|Z|gy1ITbZT%k=YG#bsUlspSR$KjklKT=IE_{qg*KEHr3~C$4v(Fg0VJqTNa8GU(^YeS@-KZs2G;t#IGSWXdtNg9b zjSU&^8=lBWSEhvM1~6C@8!%@gw^?OQa(kA-@`9UUi0My7-ff79>=WL@NCIwf7m|tX zz>YpbBvFj5x=Y%LUZ50KExy&LBu^GU%09BeWP_;6)TLz;<^7L}{+~Y@o}L|hY-tT4 z_MtIQ9p;1mvcyiUV!!jjhaY|Np#PpBiAQD+K_fQ!ee~b`(|`J%dz7&6K6rTl5heG5 z-r?Sfo?oOMh)gT+kfW9U(^S|?@v!vs>h zb7ByCWs@Z$hdysA%n(O;K)(?aB6sq(W<53w76_cGcLQMeuDqZsoMgCGK9r!XyVo@%dM>NRsxqC6p;AJiUk0>K|R&0SdDB^bWSRgwM?L@RWUPd?Wi0I z2zWQ%Gk6OUX=+lN4M`$E6uNr#_l2Wn6f5F=dKN9XhR0ZSSkKBk`Sq4o#jtcnW5z?W>$ z_6RjGQVx3NdmW}wG1s0i2u(G~J{)kB9?J0J!M>rNfw3ot7giSN)!>>*Pm-b_$tb@E z?%%na5b&U1(m;%Xxzj4T|NeKr^Zw)E$9JvodN_zaIV22B^{iW5o?$CtKB`K^{`GMe zJQ$EF=#}bI+Hd;Y*?#EdISTDQc|I;-7#AD{AU+H^v_i&+98~NDl^xJU|{6br~dVifnm(byGg?dAuiP==_V* zdB{s&ch!VtWc^H>B_&LH6$ju&A(in1p`2~xGIfCZDneMXXJN-335_u03W{Lf8aH4j zUxZwpofsJy9DMZ1f~vWRAq0P8U3!{k(57eSCPt}w5bFN$`#<>p`*-f&z1N>{O?x$1m@FAT3@t`0u*%OBqsfAERAc$o6j`*_D#ey_b*{9=}a2|RO ze4c1aS~kfMhRaX*}kt7RJwa21@;>&upo74dqiuH+6IT02^$l0?WGDknFo<-9sK$><^F{MVGZ4&#O9>$m{AGb5uD zi;6(Y#uitWCM|nS^EKm6!mkv^B*K6-9n`q!ltjTGsq3sI5#x^@j;v9g83fV~J{ov5 zHqkrVyU;W5I`moDSFDbX*`B- zP_);DJxPx?pHwlc9aiO!avN{xps(}q{Bmr~MMx`Mf~@5Ld@Lof6qxg=wpJ@XoWd7k zv@|(z=ll8#AAB;PcSE>;;K9JK5f4xeEd8Jv4qAKNBm!qmSx9n7P-OqDjgL`3k+1<6 zg!!~P13KwH2?X)>4V7$!)PXz#oHPgTB_4Nu#Vx#e_VjXh4L^EsH>pODMV<*9J271~ z1f-*3SIEXP9+5%m=D~^d`B)BpM&>+~>DOF&+80r7GiEA-wQZP?MY4q5NYjPGliQLG za5^qRN3w5gdhq7ze+zW-ntiMW^JhO+@mFfP0q|}sF zbr9WIkOH|*I_Gq2vXYJg@W8UEiLqJOg6*nW4V=Y@HK@IFy-PidsRixm>D#R_0K!AZ zr*4r}47pP0pZ=jIO#mobFP)sfuy`b|W%A&6S#a2>PFyb{^z=mjlDFcCie}XxhL=co zbgx_gF~-0b=G&+sxs{D=g@(W_nXm_a?wtwX8%@fCP*2Anee(VXAAkJe#}b-(`mw~7 zMg!YaY0R|`kb75Lbz$Jdye{j8{HVlKu7Y?SL^jOXRHK5v>rf^b>bE2)TA~>( zm?a0G?Gkd%1cy*r;~4Cntt?GFil^88R@G*Npkhj4bU31%wx$y@3SJn8n<0?tfk)g2 zgm!pnXn;$Y>YeXh>}lqqJV6@jO6I7RDA842Lmu?n{CfoQ+IE4;eBp6Z=4v%OLb zfcb_SeqboY02lR^qO(DH#OzADWr)+C?-b3qjpmw^d7JjNOesVAsll{R_&lfSqY4~) zkg|EZH5X?_sqs#i8p@WM)AFAf9S_^Xg1&)?SzN9u4uT}{*yOTYEdsbMiOSLU!J(Tn ztu=?{$&1>(8W;bC!C+JYao@ktYYTxKHjt}jgron#gMl=Yz0jZ%hIB7peD>3y{pcL9 zyI|^TrLXd~*4%4A1N;nt@dB@abuz&u2?e z%O!6j_)1_+a&15|lgkb2JnUGTKf^X96r`S(Je$K%wGt$I1=fsT^w?Zm$;Xb>8t6=4 zY+dbuAB(pGd;z`los$N_&xzbkOj)W+gF*_VL7*hEP0dp;=#Wus3Dki!w9&H~hMq@u zHpT9ubjDV-))5;Ow{lJ_-qQPVHdF zj2Rba);l1&q2&m>tv8jvx|oM)Ivcf2p{ec(PsVIJRcgz!FrGCt7LhcTwZAlDut2G| zPL2_yudHf}&~0Y}(|t>~@S~;mEfvA8$O?7B{en&r)xk+ySpW=!CIXzQ35=08Ozh0Qkk0>=dB+WPEfdg0Yy@Kg zBa$d_?cFjBFiWUfc>B2RflxTa$gPA(hvEJ1W+wFsXfJ|j=c8Pux$1gVV(KNjhSyJh zq8vz6-SDc#)Y{Cl8XS!t->5yvcJ@nY zEp2Vi3qYv17^NN*zOIxf3k8r*sBFaYciZ6ucK}@whcvCV6fq}2x#9hVXYGB*@9DzV zD!L3o8SzMEAKbvw{4XZw-2kn6p9o5C+bB%dzEj@J@<#hyW_WZ^^1|YJ`;f1RN=S?# zL)O%_WI!uIV4qAn@HA6cUhZ4k5))8S!hsQhhX zVjS6PbG;8(P;c*n7;#vZ5#^0)FJA_com$|~!TO1a!lG2rT+<0TKWU-23Su+f{G1h> zU6^nHTZLB4V7Uz$%oZS$A_wi*4cikir}o;sCu;v2*qR)1i}+G7^{{gZ(aK{DacPTK zQ7nye_oJZiOO3m@|ID>stQ-1bG?SC>e1MJ^zVAC0FtknUb1X?EF07$JAEmNlc4VU` zQ8YzJyqHtZknUe5p%!Tk?CBJ>5;^4{C8lM{OtlusM2atDcv3+k!U(CH!r_P&W5sNk zIan3e6ay}uE}S_^Eo*fJO}Gt%N}84O-0ArcuV4adUfCEIkX08M+Tos|kyMOqfrKn# zPt#|+3tLtMaEnp)O(p^{!2>7`*0={Z%pE(uJlZQdEACI(Jn8i9zIEvhM(3{8Yg@1a z^rujIQa!`bKoYFg=Z~%gMGnV_w~~q>L|W|^NJ|>$Y5<-e@5b3ITLxD3Ub5Ik7ff)4 z9;XeK=_&W5-t}VLAvp%$5~<+QbSKZouegXmd#nNtl%> z9G=?m1uB-^u;B)(v@|hmka!F6LAJe-8Vh}quV4p`8m*C5Bse$uuP&lWQ4;S@R#qg7UJ}dG0VWEPt)R(&AUONEA#v6K0JA^~qH2>ZZA2Xj z&uq>n)Y@6ur$QGoF*0M=CqG0XX)PG8nt2j5fqd;h>VRlATwGjUo#^X0Co>q90XHB% z1b3)EQasPB5o)VuLx$%9DdS%qTy%r2N{h7HBUY#mIDuDd!FRi~x5~WW>^l3AqeTUL z&bHJRFY%Ds88M!0mlrGwixjPP^fS}LLlv&q zOue!TIop~6XpG1-!re>XMi7HNauYIR#dB-dza^3|Z^Y47&nAaWUP$7?cgLrz3;*5+ z&?RbZ`xX5S18fjI)+RCFrJ)pIKS%~bJ$u&#vccZ4Hw#E3kp&hp6UZ;FrG5C``qzPy0y=3jnfb(8ougQ}2{Bk%hRe>)hYUn0$A#9~nV%Xp z1f)sB6(SQAYda2Gus62tJ=m_o0=z#lu&6EYPN+QpGn|P9EVwF$24u%oRUj9H1u)13 z_-SR4VBBx7dx0)<8t9G_(D7>T5MDiKuz>l=Ap=o@C`>qP2EYyM&91g@Hos@&P|2(2^ zy4ePS7${^68@(I7&7QZx=@N8&T<4r?sB!}c|0=VtXJt*|QExYHLjAgnz+htpy z=OS;S9X=7MZs|6)QE4%K)tS(VgoxGXt@aNL+UfDwP7hkFz+c^*F|$oebo;4ZPigQQ z2Xp6(=?=0m_B6%<69{`OvxJi*{fQaw=>y>ogyJAQ%BYl4I<8tghb z2E5RgG*uY53j1hA1j`2(PBj%sz5M|&JOn*AIe>XtUPal2dYe&6&Q_8wtxAN@$-1#J zSL=symZ zX86@5blSoOFacw$Z%g))9%xOAA(5D;MC|wbp?!U;*%P7dIQ9-)|1$t^oIt7;B^Mp(~^Y6OV~H{5~CjbYcNCEQp4amDMU}vP?-OAv6Xg^CMF4?*$T( z3LK);?RH#nA109T7728*h5I&UOp#xI*f!F|4a1JKHzE75OFx?@S~F25=wx3#w@|QezjN~PtFJ#Nk4MAO>Di3RJtpvAzW!CFY=au3 z=8A7}Dy$Z-F%^J!<>|c9bQPEa{Y%jNmS1ye9g9B^B8>}-Q{Z14_2p$1LpCR13580W z?5(%myWLZ5Xal8c@$G<>;Rz*q;hN&lIeX%3Qh>t~TWf3xMtzJb5~iJOuZ%Nki7-vb zxJ!`P7tJkg&FF1NS6G&G7e=cfNc$*Amw%N79BkVS*vX?JeTi*hFU615bb}0En)rs| zPfjkZV_L2KX&VbBQ|W|wgt+>~g6wU4d}^fMi0Y)I$15P4qWk0fnOO)Ict-0VYmF&@ zRd^(BwMwr>+ayV&Bva*A)IJqboV>F&Fs7Ll+uc2V_Qi$nvy51CKkhb4{=T&g1G{Rr zm}^9T^WF0GZ2FS}w%+Spq`4;}BG-h@kfi_6yqls3N(3iF(Jx zpV+ZEW|4!WtTNhsb@>B;YFp{ec#zbIs>iKkXn1Q^G>3U@?zA}a@e#5TH6|LGA$9S_ zTQhKeKPg9q;Nxh5po?v?OAa_FI3JTf*L)@GPA6xlM*Hu4^3mPLqtgh$Q^O#kN00jN ze)LKI5Jx^cZtv&&ANRvIZF-o%oEQmlXdF5NG1i z1EXYDpjj9$)oSmi8ZZhWGU?w{EXWbFAXJPFHpTkfe-5Zxr^I}5eRhDm?t9zo;V?6X zED=@i=?-up=_L5*JAJKvz|o<>1?Z*$V7)m`S2pKefRzi&^ERl%D>|q^dO5-#Mew|{ z^vZaDKhsZFRmypm;^WBeyHRz}v@^hiEY z96;y|!dp@80NQX`F_AOr?OwSNieaZyG!#0!XWiXx2OVGy(IV}13zAQY?o(u3V2P># zDs)7e^aRAOH zRTPJlT^LS6>!sQW>o~m}?1Xk#LcOyl;bSmC;h+jK2m|%BNc7hs3G2O03IcyX5?LqT zJ6BA}MpLs1gDSwX6D$PLbi{+YI@n8XQ$qXDuasE&eJoDgO!9f!t z5wHjG$^JS0yB4u8^R(GX{g%b342ar_H<3pc*_c~=nMJEXz}LBY@q|+nJyA1zL9Rf18ucHU$UPL6M%y?*`KrRorP-o1WuaYf!O z7sMF)-frK{k`|#5-HG_BJuqA!%yzqy?6KSeX>6Kr=U;AOWUVq z9-{0lw_KRN5;(;VaU=};4t%1%Q(uJx%O_}3+h8f658baEgnA;4Gg=B*>E4BpB*|1qqCZBKxek1=bZkUV>E^tG}`lzk~nU$PP{|P3_~}eBC891HC=X&NT(@nYfi{@-B@k%PhmIY30S1# z*NDwDN~naOuOL0Hkm-5%V`lK-!+zbTfwZPb^R;aF(lK@3olf80|Ilf7K*)g{3{C9n ztR5V-R6-e3G&cuw=9!G9EiR1ML^~AupON8#dmoPx`Y?xVm~SLI8LZ@Z{8(dbbh2-% zYFL;o^IWuw`=%P17?euLD6<2CDT(1#^{WJ4Vz;gw|R{K7vj@G?#IoU;TyLo~9R!;{Mk+tV|+k(0MZn>s#Wid2exWJ08(DyU-7 zeg(Y^i6cvt7)xTXv6=j=05?Ymup-mAgwR0Ll5p<1W%KybSpS{7Lxf^yCDXc_B~$!A zdU`d6rLgkaIT^Jfyi(Qz67J_o++zS*Mbb`kko9Uy`_ixJYR3o2MLS6Xeq{idyCr_) z<&lF}Tb&zX0FMU;1}AJ)faPr;aI0ICCG+gX!C&p!C4YRFiCa>~T1lLz8B0C@8XfyMtVI1w$m3>eBdFtHem zB3^R6zg@IUHDsg4$x(^v>}(h7^~&hIk3JbN24(k)Vt%f7x_7pxST)u~MQ>peNRIv> z7z7l7b4rF>cJ4@KNS&(vHh9QZ3n9zJ6bZZ<{W4+48XW7(Y-MJFsy>|wci)KK=!1J4 zH@X^<8dkc@bA20|Dy%yYl6|NGhOS0Kv|-mtx}-2_$c_;q{o;+PLxDQi!AJ!7-_9;h zp+RHk1`H%bi}D7_nV7PeX;|5x%V|}cuF(VhXWO|RKDv9qf5cZyWJ&=Ct1Ks@{(_=+hyD)|jM}NYc|ZYkpL383i2TPVXXh6-R<*bR*H%}ZJi8R! zpTaBXYoBzsb}c>XQrb&l;U(v`Neq{<-}8i*%7djzXzyo`6ANQTswbP!P3IFa_BQ1ZDwS7>JYRUbIiAYD_L^&(Iu^ZLmANYFQ#8Q_bj+G`7r@TERSCk2wJFE3Ua2j z!A)piefAVVGPLLL#g(iuVh18s$JOD&q4JRU(v}F~MTkEt956Q;^2y}^^ zn$9<+d3J7?_~P2F*BQLL<^TkM%d6|)1nfbm6$KA$(=4yj3Hsyt(;K%RZJ*w!0g2Z zw6Lk3Z`ZoC>#{pAL~Rv9!Wj{hjCsTXa4x7;d)FF+P^X~SvLcB{>Q4+%ndqKJK03Kz z)PmLyKFM39LuJ+y`C(fVmhsRY4UAIJYej$0_b!LPRXXl`>KR~9{(=fI`lx?sb1d684aYo|G)_*8 zjT*9eOXntroo6P|H+ZP z1zUWGg92wlISk#HPmA`CAZ(05ZPz90z>1Bp(K$_~fBG-9m(H%vFSz{+7ysdCc! z)6J77-BUoo*{N=Dq*3-yFGK)|lr1*rw^xUW;i98c1gVOhE*t$1>y)xgQJqq>RwaUn zC=6h~=sb58YKt)bWiOBwkwlSTBavPLKoL%!+pj5VFjhs&0496%Tv9*FZ{@?q+F`m! z5sV=H#d6)tWR`M3N91}It*&^$DTU5RGA0ahzngH?=^B|)Th!P3=Fjw-XRXP?S5q*} z4h;@V0uk!05K7M_osD^_N?(Zarf|8?U_o{TMlc}~quzQ$1`+y^)mZ@>k@~Wa2e>2p z7qeS*AmZ}T9F`ZbMbe-x7T>~vX;6%h$vCIe`j+du`KHEci1?#Zibk8=LIau1vH^l2 z;C$Vo^p)}x{D&VVsmtmSMKd(4ExJXXQimiPhnsClV@Yu)Ixl-j4>}q{sU?42Y6mtN zt82M5q5#FqCr^&`cD$D4>7bk-9%w^eow%O9YPy&}RbZw>U*Id&w?lMD1f)JVY1FfX z{#}itQ>T?Hk&gF8z*gb)HJ7ryhAu?yGgWyIaU!CeD&OE1F6(gv)Iu8&3?!|n0(n1H znLW7w;QqaP_j~($2eg6$YNF~&XrQbC@Ijjr-_V)(bUD6CB>5%p#-cds9skP19a=9^ znn1`;mKs`)TXKlnSt*pJDZ!XIjpxSZs^b_M!=zlOHP^XuvB|8Q57TV_8*dexF<~rJ zbYOwa3E8#mU4>S!2ti0lQ6h8qsA6uaFNPISIETb+tl#uG!#{95_yhrh6C$W~|GmV| z7owCoX=4;}H<5w|KoP^1C|URZ$-4ZJ2!;j^3$`OI_LuPA5{=JJb~9>8E{xx8wM9rX zxYBIu?J0?4Dh@y*y@3HInbA>{tf`OO@K>K+nq&(yqzPhXLY;&}C?seWc5Qb^7!=<# z5iM7@hf@@CA(#wjyRkYO{SS4sAD*_+0hHA+nnp4ss5Qg}Dk_Y}p~2>WD9AMeFs=Jy zyOOUlTv0EoTWV;6bg8cyBI6Tk4n-4 z*FYhsqv!DhSuq0xvn6{VYH#raoVdbe_w4FS;s6TEY_IdMOOI|BFnfJv++KN3VVRP9 zoxF&4s%ddFutXP)S$-6>C2)!afWb7onR=&`FOVt1Dt<-v#ql;CkD)pLY4j#CD9wOY zja8bP&-UX;*GzK+qoX#pfsL}oi5HOV5dTt_VT&S^BvSlKFxUw1V2)_Ew9a_< zxB-xcze*!GJUxzT30LghV*p^ahS+HFyAP-84jaZl>A3@0IL*#xjDXnJ;Yo}D)|0gt zZvWpNNbaoq6*9@;Dh=cb+9K|C_S;E1qJkI%Cb?8O@~siF;)E4~zDVM~x9{!B4n_OG zJP<;EiWBjqE_wzz{12uEci*`%Vyl!U70=q^2jH3^O1mALqH&_f=3z(73o-)WIQIuH z%OKZKQ(4BW<&IDa{{%z^kli59#@Jsle*B*R6lS zzKs;kYH~>EA`we6a;(rynJyRF1vn7ctLr)$y zUt#NnCN5B9V&LK9kH7c+M?d)9Z+*A-w|YP7Ib+8ON*;ryF-3r{E*o2dKO8e*V8B6f z^jdG|GI|-{iY~rm3QtuJ55TjXDrhCXupZBnd#yB^nO>MHW-C*pci;cuo(adXCJqiE zW~lwrDy%Z2c-7i_4Wdh)nv4=NT;AL}p#zC5rClkdQH5$7i2^V-C~jK5!nl@X@AP}Z(w_4y*nL~k}&uI$#W(l<3SE$(vF3d3%AD z-rM>QGP*QGqeK*>Nj0sG@Wf6i_zin^RVAdFH%V>{Bn5rYF%=%QUXCs=j=OZ-MPZBA z>du*!5*?Dhr7PFz9^h8$2o{v2_LHDKM0tuX+p%TAys&gVxhG-KLr>R^{r;2FQf03?MStMaEhUc8UVnn(X&Y@ zOoJw;pXRlDK|Gj%fr(^Ir{rK^ztoMKW}d=z#G)mGoF;a#?;MO24*5z|yd9}@d#D)z zE6Bjpnq=F8)M~em!H}j|o%U`d@mWg;8$=@zF&Hcu0WAG8FkxO$Db^st!U9OVVP^8| zqP-IQ-vg~x`j-BshOP0&bTZ&*MhN!l*`9PeJDZ6pyJVgy#W4W9g*Z!h_t?~ohm%cU zgUKbtDb?liUebt`#1RL-?6oY64~^XY!QBUU@80>O_fBtr&s+6liZdP}1Ur;+!F>GW>y-oN|s zktOR79$KV2F-S>l?9t;PEj`|@R{vw^Y+TcrI+>2GN0Ss+>Uk28nuopJ%CT8-Ra zxX>4dftJ&Qz#SC9#_~owh&WZvgY>G{`B4TwL!R9~J@!#qg6&5bXSF%^-kW zubeEA%;%|cIWnQeOQdLo5XhF|YV+>VNEUVVAND_@gJ*IrRk+T+!jyD~j(sXpZXDxkJNWH<| zLk-v6wSD`FEfg{BAd4+FkLbFVKY|e$VH9TEIX4+4+BMv2_3iwbQl5?bugFpwZWm+o zNbx>kB@TPLLmbpwjU9yc6&sL~Rpj=jngDEAOF`p!+jQT+GQt4z`Qa}#WpVsAQ?H%g ze*V>)7ki`@-Bs!;(pIn<-yi@e;xWEXIPL7bTcr*@Ht>*kz=L~M@4esqLGSxLSAa=X zh8YX7fR%<%92@XvQme`KPr8=|;&zLp|5fitAW#;lJwe{KjuK8dl^sy1lOW0*lyfbX z?}~+~JwG|}@WH+Nv??eenP7v`Lu;m8+)-nXuoYxp;Bvf_Iheeh_07}Ej_|0+Y8Raf zF|}yw0ucx}X6364xYPF9p*W%Y=+-fTY0GvA1~^l9i3xBSUVGEjNJ{K9vazCAP{tme zyPTh&6N$VK_LcNs0|L$I-QNLaDgGkzrPC1UCVHJwE*YX3Iy5-&7*zNen{!w=;P6el zjV}t{$;9P%2887aW$s&pBy7n=`TjRbUgLCscztpxT9$ufY-Di=OiUG1ywU`r+}5nSu!DV zo^Ep6q7sm&OW)MZ4lUk|Jij_Yw~LA%Y${%V-E!chwqj%;m_#AfYIFk^UDQZ={^M3P z^D%zP?D*6?Rz~1HKAaNPB^VOI62O&@5*Ctx(jG>bP!7sSOZ2eGh_w1Pf93(UH>tvSH|thwF0F?XwH1CYQT!=Z~3Tnw!3dgjV0#G*hJRyadfa z{4Y)ti@<4Mt(Dg7SmImPHY9>{Oh(7_q6Wg{9UUGWgVa!WFDlPg_0|^U>;Yj7_yn?> zlf0#vBj4RhGE=G}?;R=!oYig4>D}qa2mn?o|FYk*o=Mn2`aW5Q$N+hWf(Q%9wdB${ zZp8!}5NNQsWvPze(G=CW$Z#g^@XK2*s>DeLvKHd6nU+6*zQ7bTWDj(696!WLULClv zgY16>!O=vLrn+K|AN$m!v-wo~PQgu=szUIS7eFFHSmG$j|^&S-kb1y(NW0M_k8IuMLcFSe&qk*BC>5yb$Cp`}NU9^Aj1+utCf9sMrXhuh{sq&I6n(Wz`?u#1C| z78vJbdmN$%2#+`OcJmdMPWxkS#{7~kNz*1%%`$1)&O$kgg=-28;8);0j9$AY{GG=S z?}yff!65@z05TNCS+V@wzORXSO$yVP@_982xV-{y39urO^QFQ zvLU1E?PRzZ&DSUbf@{Dpe+m1%NuJvOlJ$l9A-1_hY{74{1evaQC3j@Dh~OE83rW(K z%YtKRl4_S%90c!&&DP%2;@{o7-G%ZYG;P;u{=@=A8bmz$W06Slvk|*XX%U+{I_Z`< z3jtQRsR%@n59_)lfr|^$K+7tWr>jm{wx9vgy?JqUc5!z5{JP7L17n5$u)?{P&LJaK zmG16!uU>xkX;)t3xid_H?@ED4;emeRA2JcWeI94~V8_}ExOC#W+%ylWQIypS?&Qz_ zUXAv^gPu!sS;A-J6zHHpxDD{o4{qBT!=VI$lv2n=itRroYa-OhVT5SKN*Wrl7z{jY z3+w`dsfLz-bb^4S%0>@WHyKm(YN}@%JCmcf4q$iUaw25m?x%Eh&8vTof;CG@22A4} z1^|#A)Ib5EGHy~T>X7bEh=nr3Dy*rM7A67FT&)8g>N#qci05!}>H zEj=2ESIImo+H)P0W`viptnp)fJ&)-_dxu`JR!r-u>N@-SP}tluCFA( zr`Q|1+IVk10#5I94pfpVY^%k6q%h4K0e%hw9fUD95F>~Px+eclvGJA-EQ&jn?$*dL@Q;0d@xkv;3TaQcl_k~*=JvTe&(#jIiZX*QlKLe zcnZ~mU*B#SLW=6`{)q`S6`W6#ESP0WPr{c+o?C#QkMs^{5&e0lV5LvuI55Zwn>@(+ z9D&CWfyZzG8a1z#9};s8%_;h35^h2=86&il4w~khgm6Ul4CWH%jE1kiiv)eo z!??>lZ*&%7*=#^0v0{p~115>zroCX)@4Z$wKez^|&y=0Omt*9nHqlwuN*6trw+*w;X!E-kkAOS%I1BZO!4UXP0N!S1&((dKtzkU@<1( zB5o0>&}UNIpLDOEzW(8tS6+nX7{_g_ASQql1W>VW{D4pm*E1*>o$DmBt$8N*o2_yH$)L%tM-k9C~#Uh!Vu$i zNePu4o;|-l47Ewl*C~!XPh$^Glu|_ulloy>V8)=LW&tYdUSnw;<%%_?;^y>-6S4sC zRH_$&n+AQO6H>`jX{TqxnGn5kYHoaz$atCRU2oR$O-=`N%dqw=_Po8sfE@%`~?u&9!o+ zs~(QLU+;b*n;(?ooZ=pVYSzgJBAF7>&)O|WN`xU2f`n%TF;j@#m}d{I2vY4<>Q<5R zgk^#0jj_7RYCXkJ4l!pNwzG0eah&!%VBEbgPZUyfmAhR=NPa)UE?yJu!Hi4h>I(h2 zV8Tq_&T>MrWU7$7IeJl{6pBEJ!t}fT^l29)x?3DrfPVH>cYo)^0wq=xixI)Nk@W0+ zN2hKks#R_9&L}hj7 zeuRb^pr%3WfsZ!`#|JXo^^MNu%dfwFaa>w}cf(gwS`O6Yv-<~D@`0qDpU9Jlw29NC zghoeu$9hM4v|UmwOJZC&H!|Y{YZ`c%XgWqPC8KQooV@7x5cF%7rDavun7l8MRP=X= z)4_Ykm`Jh+afxXZYaPTcxgug6Mn@xx_{^yaR=c=+ud1z89_UA*(38K$1Vbu*QPif$^KEhL1T0 znV@3Z{dd0K?)I8#t{O8;D|=<^tXrfUF07!vITP zbKOr;2JhRj`sA}Gm&F;@ph1#T0|t~R_FJ3BU;l`Y;pK|0HBWGUVxK@jLEz*MI~UKt z{PNimcDSTP^fgj8V}VdnCR%&-_m&%4bf7byGp*4IKekq^bb~zv9Sg=DDxOL!gR4pLqRouXRV;LI zJaoO_cp3*Tyt%&W60JWOb?}J9m!ftpKbJFw4?;8#yT^*EvEjkNAn^PTYf7&R@X=b zSue7(t4x|=QU2A3=QuJI^w!w1!q=TL2do;mk_DxBuk4FzTzLUzqa|D@l&Exm(BAcn z*Uv9cc0(c&hHPNTiZ;$EV^(SLxm;?uSPDzj@j;7SNpy;!g6Cr5!%3DtP-e;A4%aq%u0{Q4!IOxu zreUM^H#4tFpTZUaxl?<6PX!#hNbje&Jf zudmNJX(Irq8U?Bi!v|iw_7M}2iSFOL{OrZ``BAbCz7k$)MpZcL?5FE$ii&@#?B;T^ zhvIO_SQU8I6kSFtlZoyhT@W7P$UA-eZ#u`fmkJVk&g-@Axjo>p%7I^M?eKO^jGT#8|cAyuUFA_6p#iq9O9Yl_2@p*mfK~I8*SC?X6;a-miVTbLGw@4RF?q z{$LJq2uT`!+WaEN;Ob*acYey!MNcO$>XwWM7@j>osJ_>?SB3y052Z*=Vipl)f0!Cj zV*SDOi_cy@xi~7CH-1FstVuR-x8c|zf<{gj>C%~a7D@k+3+f*2{Xq&YaktgUqE1QA z9%ceue($myboOv(eeLv8t54dx4Gl_ix_xx@;?0kK`qk}aIH@B3*%K40@d2m-F)$TL z7{xKm-QyjX%(6HaadHw%Xr$+V^I@|sAbkw!Rlcwr<}u4Ep${f>c=q&WFKIV-jRV(48ycZ(=k|s{fDLWzr@P-w)mpYObc(BgC~}veRMHOW z{dw@PJX#jI1q z?|)RBZ(K3Ri!;P^WVEyHX~9%gtfUxD-~3v|%ACrCdBIld*!og_OiC_=*LZaY+gV4UQ*N5XlA8 zNZFvu3(GQaPvVVSRU;Ed#M@>ss!l;*H^odsoDW6U)xB^_;cCZ-+J`5ubRPmg}4A#`qpdH5%E&x-epdWn#9LOx+&^ zUO5Ov+Ky1ok3vbbjx;=AP5xZsO^_gWxU4Yt{@6Kl6#0zdj`7R~=eM`l7l)nJ*$ty? z9^HKTqu0;Q;3TP!fFN@V2rc@uQay7=CK9i2Zf>uxt}~O+HJ@fvcX3h?aQ3lRkNOUf z7y}MSE<6zj?gcN>lU(X~7Qi*uA;blR^jhsc(T)`Q-`T+K;pK~`RuSP5SZ?;t2E3Z0 zU2Oi&mYoNQ!s)^)uS@z{tPIT3d}ji983uj=1P8pqAmS$$eFi)Nx>rQ{d^+Mgu3y@> z#rjhx$KRlmneMRQI|+l%Z3qOJ-4Z~MLqLEM2%n^IHKkf>3=n|GHHG;jn_DZ`h|dZ8 zQxiHnuj)vKTy(y)nT81*8Ma6(%+Ojcd-+#blQ(v=-$ zzkxq-z+P-znWFKBo2ItUp4?oX`BvxGQPA$3efH+%vkP|xtCOK?rm7yb3S((c zpOM17)H|?o$KH+XYB!me#gb8lDTqVWMKuKm9kk=4KSqsJyf0Yhd!W8@I+kE0?`K}ZUT*Bb_(S_1B zUs=*8b~@?V@sD9f%OMd6I3k9F`@CR-tifR`YUU6_4f5QJS55?NU241GkCMW zMwygO=ZJ*dsrT&#J54@FqN>`6e;^g8@YNmRZef1{+#L5N+acISm2b7RMc~GF#YbLQ zeQ)tE4HsHphsY;9YHZbXpllrH|5450cO;D}W<^X%u!(&z|(X*tt4CfA;Fd7oT3A zoL}CMC}6HYqCFcSYLFrDKG;WljFMobCu4jdrla#G&z?Vj_VVQy-+c4t(`Q%3#o;+% z;Z$U~ym@j#_DAJTPl0Y}QDX|#+5}LFn z{uak3B)m&TCQJu1fZ4H;OpB`$36@JoQ7rMhAWXq76oCPWP$Z?^694Vp?&bA)Sls#o zd;9Irt2Ud&Eh^sJg^W?{q3q&#NDNlwiJr--9167?+Od-0lu43#O$xZi0@g8IvGz5_ zqZAXkw*$S8%?2>y?BpXtOYClI2fA!OPxasUlVwp&>m`|qXNsTjfB8xC;^)8r&CAnW z4W*KS!xxtIkZ*11w5!FKX*IKHM$=w?{`#}mw^x@hUcdO__08$!^(}~@TL&;Ta2fcKhu1<(n@*|MbO^>#NJl(wCTYyt!oonevSb=rW}C zHNP7ApyQTzQTkGS*Eb|Thw)KzbPq1&;P{K$FJz1&mtImz1vn7cDe6t{EFc3Aa%UnF zDP8?3=aNiHR02~3aFYBtd7Y;9)>^}YLUK_iP)^E;&oZp7ZXkId!#fSmy$};HJTy;> z*m5>*W^LZkz@jd&M@##L+wpCk3B*V??tODBCl^;{I*#{M4w!hWS=AMuk8;YED{gY1 zTg`&2?e-eion~CSF11->i9U3ienM0xSOi#M3Z$N#+43Tct-baU>VSevNXd!XD=&o+ zrBqzNWZte=E!5d5w!z?MC z63t{7AnG`Y=$`+z29Zne9RjkvX!V5NREWeDGZcd)Fisa;Wc+A zcwary%Tl+#xJ4^aCWMT@4M|GM^r|Lm(iE=tBuzH4{Ot9oFP=Yt@$%)17dNL+|n*p6Xy70}ucy^iOL#wU}o-mk!T;^IpuMAJ&QEK0EGMPMtBa}f8Q(Z$CV~_o( zg+tn{1l}AnBj7QhkD{#~CTWo!DaYdVov$m1ry~3WlnG$(mitm)VLB`5jDJb(=5YM! zjwKns#J%G*Gm{Yf5ck!LpZyY(!}mV<*erX7h^HqFGmU4xNo85`&Ie0qrRi-Gazw6? zAj}nxK7KSdS2NEYjx5AA6dI3|Pb$lWL+JnsJ%V(dM`Wx&u{J^-(g;SlG%+?a4fEvv z^g^t`%%W7J8ZS#5#_7tbv0cYm$b{vf9D#b{y^Y@m*D$e=4&?&rk1MH?@$MX6zW&MY z{Z7}yvuOCGGFsnz^5R(V1v1HTd3g6SV}0)P1#Z*r)9Z7ys<%KQoP(E7*@F5YN(LEi z3yVxO7IoUv1`w5SJujYr{@JH5Si!TW&#qBV6qb%!648|m_MN}YPRGk9FFwCL%jiM? z(Zd~b1PBJ3B6b{_JGzJSq^FPQA{FpzgYRWLD|0;jKtR7pIeFhQ(qEZCFe$IrAp%_e zYif7D#}8?-sz`X-t;Xvh)SRfF!oB*Ph?wx=2H5{!rtZVZvh=tPd^heJ{b5=zu^4~@ zU>q>s_4Ks&uDZPUUEbSQ_3D+c>aKRvV=)j33`s3SBP}I}B0-6}3Q@7rMsR~mZ74eS zcWz-fHpcgKRlR$^{4!6TJef%rm&l0%^~*G3%%DtpZ5-{}E(eqGM}2Kg*9;iA#sswF zw2&~_YMQXyGj8(B=;S==*P8dtK~an#IB0J+e!p{gY9&b_U@wOYWD)<75pKTLQs593 z!~+TurN?%5%78N6ZZ*-Ye9-ZfX3Ab~iHyrQ$iZxpDmS3T0?a4wXlWlPXFkjwh|T2- z0(ZHXP_1r%l2Sb>Nk*&tpa1gP%?YM7$q0)6Z--{K50+R8BUmz^#wB$qcCW6l?;wZj z2Zy^`Tf6&*yU60b`wtJ<0wZ7pNW$3#*DvJ>%~8ij@IPY{Hp|&}Hv5Tvz?ldT4FK4Z zdT647AF{N)x4pe%CLSXw3+Q~Xf(4+4Kp&A8G_{a*6;p%rYqh!5Yh{R){x7KqMuY;# zPD~t^hjlhOI|>cJ(f5ev%vB~%iyB#gMv@7D5<1n7$-cPYL?*jTC#asL5HsYr`N7Np za~}4dZ9pdRCSk%!qel1SrzQJ8J+?5BdV|_H$w~v+vPHg{i#yv?p{16#FLV^W|2i8` z(@WT1b`)lkH}cAl2=NdrR7>{lQZ#qoWxo@L?JbJRJbo{p8Zl66k%I?@Ty#_T(o8V& zC2z?6xep%QE$2k0n1foXs{uH6H!GWkGmU^9qXrGA1FO>|=8PK)dh8~p7jh0wE-tF_La1Ckf;u6d$!1MqC?u&UfYbp$Zaj>jYp6Tx$pEg%) z^hB}q)oywkaS0pY0(8KF#mcHu?D^0Rh$b^}R`w2{1^Gw3O`JroYYYIp@-E&_KwbEK+LNHjnXAG7rwjaQc@c+h^6ku__THma#)2+nv!+nq>8Y{S6+dtUd zJ=oPoD*Y@NR+MPLW@5U18GQ!Db6?JNX=7_KVrg;G+pq&-a`K)ezgxqrG$j2_-cR5- z!c7QtNNVuf9f8j9?_4f!h5_iQGNvp?A9NrPFHR%N5F{~w;c>pQ@K=7FA!KG@@WBH5 zGSJ?SraG3o%}lPShe2pY4v7DmN8Z+?&7@bSD9EmCr(H~nl7<1>d?eZimv_06ijJUm zF;h!bg|a9W=Zy7&PB;L1LM(zJo@u7tor49*kSW@!Tk#0P`qjoWg3rT_gb8p(GFRIi z+rxb7IRGK#HM0{FP!n*HT2G2>PCps5fAf4^O(p5NmL1Fv>Mnbvz-JyMZ+U#I0)r)) zUar=6QGv!5DJL6dZAOvO@>;Ta_jV1eTddZ0>-(E4^i4<&-*j%Kzi-k^IoF>)5n)QC zj00NV*wG|pGLUiClDZ&4CIx1|1ItCVS3ayRU&A)4XS!ukMfVs2CdDrX|8&?DK|Yg| z17qF~k~7(#&=3Bk>L+iG54650wIkvx_qco-TvNh&0HCtE@j0EBysjG7?~A~ zqpB++LNtQb03c;^4Jke&nL;s_RhgVvxc!V>U^O6q8bo?*koOt%sYywUhs#<60osB9 zo>%@LNN@+R3Yg_TO|M0tKX(ruqexqphkf!1$RlM@Ev-pYWC>>R1f3h$-`_^~v zKY3hR-+yrO;N-@Mr`x+`-L2O*z=y3J$^Y_N9Wg2sQ_Z$9MTr6IlNCm=pm?xM%(lI~ ziWlzWB?{(*_)ev8*hcaUJ;SJ^8ZAE<-Q~^Pdwd;8ASgK3#DwEQKyZngva-lqMDa|;9xmCUj~svT8@h zg(*Lz(^snJWJw?fD0QPTd790g&CQ*)G6;*+Pu#Z9r6~(WOeX@9aitg)ZnunXY2IQ77@%BR zu+KFx{t~c_T}b~>?X=jD0fo0(THVMrc6B4`jFR>x5+n(T5x`;`{-XCaWSS-)?R|pd z{y)jANRpuk#8H5ZiAz!H)bx=g6$R8BFKdtxo33KyiEUw}5lCB#(O+ijLbOZ4$;sV# zaO%_8G7iV7^Pgh46r4}acT6nOy8;VI)A<2ipYG#$<(_yw+TYe;Ty6UDxvdF((UX;l z;e7C9Fc}A@vwwShusIfBS9~lW1c#bf85JNwP8Hn%4EUoMnGNNY#L4}S!Y=Mnev|j75g_!|RM|`dyPdMCv`03M)wY~c%_j!T;>f~3VLrbLu`l#qJnAWL;34#wJd1ns1YAT}@RP>PQ7W zO$cB{5Dd%{X8((TP~q-mq#U`2gOY)TKTm#06`Tqbd%|4`C0@Ily1x<%xV>XFpofkY zJHn@2H!*Q?d>~PkXU>cpd<42O%4~Em>mkuf7Rk`^#wZ0wj}*5%)fAvFJqqedE;L7h z%V&&p!Z;N)6%Lf5IF$DiR`Of-VPxFtnH=g;y8ehkb&&KPaw-(7r9wtcI_S7$as(uB zE3ZgO<@xwCV>FAEf5F#Y9nh9ZOXV8zJio-jqG&~-+ZY(06hZHU?S*6!iK?go<2k4q2CjIMC^!zLt--Sv`k zj!m4LGG~4>0z7rEjD%R(qE;^H4`AS)8!B+A+(<4k!V6Cb7mR{4C@Vunpil|?9G{(; zVY(|c*llbd?XG*e@UM_jz;TLxTVa1d8P9i2%L?7N!KD3w64EC%bLX{GEEkK9mc%MI zElNj4c^@GqHMgxy@Pag)il33G>=Fw9M%q%)c1@|>34I<%lDxe1!oolBQ_6$Vj0}hH z!;ZOz(LM!4v}H1H&*0T1lxUEgF^|Ee7P@c~(x)Km1QRHQi_yKy#dzuIoKM1{=a$O{ zb$AX%l9KMpVy5EWwAPyp;z;L*k}Qv(`ard^`dn%y=aIEor_t1SyNU453qWItH@W35 zNs2}i28e^v15uvLiSw}bDO=w^GUZdPEfe?$&6Gg@$Xwy8+VkBC6zF^Cv08$_#;5LA)6I?<0eq$*xVb3Fwz^3)sF7LY^d23ugr7 zkp`tXAON})9O3v4^O39gPE>bfG)ab%nPI;4ZbUNZ9;YcGceh+9ZT3dQ&1u9KgChw? zOp<->P5RT9PY>O>b?tUkFOEEOScul*1UOCKDDfjnhwmzfbkAc1A{jhGr(%0?2`Hom zxd{`z5p}4N8%ZWgv-|dfuc5hR8ap%H7A@9JLb<s1qvj|RD4#10U(Y`LOWX{$#zg*1SbN$^n&*mNjt~1YN)xmMpDIPL1N{l9N zV7ool1d-mCPjh4H=YDF3=NmKn-VKs~712 zAq6`S6WwAqbBhB!RP@HQrp5B8<}mW{YzT7Rd{i;vGcxwLR6ip1x6q$#pN#3>sHFHY z;T9rjl8QPGSl{5Y42sa(mfDZ+ytc8kfAIL>j%l^K2YXwq^AkG6jmeQo8I){XOG%-C zNzO4kxCxxV?1={p$S-#-8c^Xci)LmQL-MZc;}ot1QK&1-j2y9Shbbl?EL)k@-4ElF z5-sD?mltc>#wo9rx0Y=dsnD3#NVXMU~E(zr$ zyKxYSPe5YuGjAyc;VP`}wGUH&Q23K&SsFrHjM+)jCf;vsA3S*U*{25w#qRLo{?6L+ z40X3c-e_UA($M$<;z&aB zjSY;9mq)iyEd4ug-zWFGeIXPH=sFCy*~5a5sVHz%974Eh+%?7Dh`C477c26lt4WBR zh9pyr5^8+JKrf#YA2nMUv!jIj(H(4O128w~CwO?%cw)3rL$T2BD!aP?Rk6}@eEyZ? zomCrVhWG&G5-%)7idX2Kc}xc29FkZu*{R7S|C-cA7qxcx)Hv((*b^g=%c1aGsk6I4;bcToMJodbD#)C1f~= zm*j`e0sh*kg;YK$Mh^9P73-V3v5qL8f}&(XlBOaz4<-wf8`IJYWYn37U>Xw3JTO*w;cn4Fw@@!u#ws1oE=nbJ(C>emfhvC6|+D0b5WiNLqXsUr))hLGXa0FmzY+g zZF5(<*%p~9Z^D8+KpyBFB{?=1e${@)`Tv;WrNyBMMlNS)JC7|eWDuwv1B)jaQqaTO zvu6$iNbxp%UwPhmT|=xr`wr*^%?5v0JM!o*s1g6cxP+N*td1F>Qf(luy5Jb%FK`}sUESu`wsT9+a^<&WF;^0hj1I>>3{ghnQ)i`*unwG*~PUH*6Q|4Q~ zC6doECrjml=ReKaiKc@7q@hmiKeBiZ9#eCyy6%U?Tvo>PuVQQ>w#{xwcvkM6akpw+{vT`oaA}&87%pHu3rA_YV$sYQ^S# z`>gICJ$!P%ra@tPAUf;p%{7)|Vti%GGBtVZfGt2&mzwyR+WNmUd}765?ewbUiBy`s z;W&3eG+w~1%?dOpLP=TM+6%afWLntf{3XtgQ~=FWxUZCnORIjClm$-Z9bGmS0RELQ zMxgYLG88BMEWSS3TFG@3`|H-;Dcr$$XVxPJGiIS5DL-7hC6M z^mkCE57>H1&xB}zkTXFmX*B-b4J0N?Zd5EiCAqSf&Wa)q5?bY;RhY}YdG5>1q*aUV zN9F=%`VyiBIaAwQ->B5BojCaVn*%#2IZhd!>Yi~GnMG$#IUtnMUZwh1p<|i4SI?7W zs!$$Xtlv6#bhOO{^+vP+m4t&Ge^K$?=q$aqz)GlKJZgANBHyC46# z5vDUxm^TJa;s%oXB;Ux)5JeciU5JsSM2;XlGN=3}%kn$WN|LZ*C~Zq+shVh=NkA!& zBOYNFyo7pDN?7Unk2)RCkMC2L(XQI3cTbKDDG^`M;um+nHJAcp*u*<_(g6DbYA^Fv zK-(iA$sKVIFk{PVrp9dQqbYf9X;kD(2__yAEW~dW6Q9y#L7+~K8LySyMcnv&U4sw* z{owJ@j*WC`ImQr2k5#%SI5RuAzKWcjxdn6j^`&{yhjUy)-`95){SO`-J=mh*+~3nr zR5r5Yt9dx;GN)H8?f5T-E)7DT`V`QwNf(Q|hYyYp_V#=sM?fFDTHQ%;VCTg1N6!D# zXP!a)Z&iv@OPnvi# zJz1HxlVP@x@gYYt(epQ8v+Iuq!pd#dqTWvY+S%LL-r2hU__L?`8{&3t%cy>yC4ESU z_iD|WN`It%bc=99+!3Brq`~b-<#)FC$@=a;eDZkjptuk2Z<=CSF1fF76!oF1mq6&%?>B{=~=6{**B zEj~DcV_ip>XN)vgzGABm2r9#}np9d05#-y0gxa&Vpo{a@ zSkppuKEN{2-Sn382GmG~lRqqiF>C89wG~WDX#&Dpygqp;aLXg$I7J4XirwWLrpnTn zgjHE3;63^*+ccSb+Yg=?23Sw6Zj&xUz9|alAlX7VPeDsjr>-KmUaZ|;S@d?(8Y3)E@0`0$Drty5e(v*Br~k z9fJy;PpRhk?-N!VvMUDwcn zel$ha%GjJPFI_cyQxtqjc%+>mkc~mzMeAfsGgxfL++DIuN}rX{2n-`fx?)^eCC-4ADX)bqJw2G-Apa^@J>QVWBd21gddH=|{6F0nuL;LseGt z3*nwI*QUVxw>a3?Hr1pZ z$uT)6p5GB$TU)9l0?yq;R8ADwN!{eJ!nr1AD~;c>3*+`irm#_CV={0V8HQ3_EEYapsZJ??3%@Y{WSYz*#r#mh(J$aF zaZ0tdMc*Ujbt$P1aaWA2kx$_fF#@8kG9Cv~S^x!*PQ@Nc81H~xLY-mxl+!cYNmsaq z0qdl3=H6#m23%Em2}K;}lnEg8Dz+&L$ATZYbuB=%+C zgGdY_(q4^1ZTUtjxMX4|q0Lx_Vk#rF6VSaoqxg6~t z>ZY&PZt&52J>4=dGj;y~=;0~C6Qi9}{^TV(tY)=&nq<;#i^munXp9XoBK5-bn~g?% z2L*IBHEC_cRdO|Y9n`WYLWTtxXY44wUZ^JosZue>x1wOKk$@}Y3xl4~7(OmlGhks% zfQ&T1y_g}IKuJVVrIaP1qRGwrh1bFKnwK^ z3O(Ke--dPFu2*W`@WnRf8Fjtg6J`V!I+W4i}#oCe=XyD%H+}h63 z(`QefK70D`(W4_qaDN{b+&%H%W5yYb&c9fb5|}5lFV)f~@3aL5iGF=&Zm4HGbQ$hS z{lmO*2@?1 zJw0g&#>|EE$`*wrQHTjg?|u2h<_K_q7HP@j;co;+3^ z#`VYU7zvsmxSbsB>ok&DBcPTNNV8b_I{`09HWWRevAm=}uGmZ2#aUpyDOwC33SQ+U zB*5OUJVDYG&N{#?H#*rM@*|-GpNbO;V-jyJH>|GFX>P6oI0X%j-)C_pycCMl|2Bz~ z7Bq{JlDKipz6vF;o6aN*)~C;6Lh(?~dNDs1iGwO%sL80lmym)dS5EXqPE=ieq1u&d zmiai##p)BGAI(f*l6@1)oBNBS{nPVS13DbHa?$o=x)BGx_28>-_MP+MGRv8)ScEYX zfnf+{YtPsp*ya=;L}1cWVwVOj*7oi3Iy&%vs3gEs2)SUs_m} zMN+3ohDaSKFJ%mFu=AhKw{ZIyk_FtIb}Ze^AUh*$%_m9@8cUr55=gbP@EkzR8R0Jd z&`i?ERAJ*n{RXgQq7Noi{+XRBCjX7?=PHh7nQbQ}s~;qo74)`5Vf@qoHwTnlpiz+1N_R#*|Gma}7$a}A% zzIp%X5PajKUAtrM(6@@Mg{QXb%$E2>{t!&*dUe)QRh3HV;EI0F6#}6R0FA*&Evd+4 z92`Zl)=^zMxqh;Cq7&A8VWT%NfMrIaivg?KZa`u}7=$2N**;jF7+I>Op9zODqZnV` zP{l<7)(^k>*|7om*uY+NykVZhVP!QD7cVBRy3uPP;k^-8Z#r|N_;D@YX`_hejNO9r zKlNRY{IEbsTH78j!htr97H3%8Gp`~sJKNP+HSx0WB@Mly7Df4pc!IzDJvQK* zl{Vl8IICjeuN|y#n}c25*wFm3yOZCRF9~|k--|Zz<57*0igg=~`28e5ap}p~bCvjo z0D)x?YmEz*W@K3iV6myiygT2l{+-PeBW#ZCA||!Q3j&4AcezexQeT@(urB2*8l{bc zZER}l%7VB<4G1b6lo>1Iw}CPZ!0C0UlSda5~!prQw9%*qnCo?I{WJ|-4cW^~Lx zPh+$xY%-i$MkhBXoQA>tG~N$puC1ir#MEKnT1_32%|t|JE`fJCYVzv8PC-Qw;AEK@ zH&3e>`3H&Cy=d^UM(M>FP=Yr_-n=8bY8qRVeblGBwX@3?4AAnGAuCi)W;51Zu)$UB zBvaK;B%4UtoR$!T?C_fXov`kYChn83fG*YRrw)n-C$b;=1Ujz3_#YsS%XZ zzYJ5HVT85U&#ir~nTHtIo;_+gLDwNT1p-O$k4qmy6pmk5 z_Z)JPe1DsBVU@j|jYTiS_p8hdOy~#fxZBv+ITAW1ol|ooRFYosV2QYVYGrz#D1BH9 zNDREBwyV+vrXgyc#T82NN!fII#yF9vD#94&H7o3pmMO{_>f$2lh_X%*@4%@oBg0^# zr3KF`UhA@3vY%ukluy7wE~?)b<8^#;uwr~~c46E;pJ753{osBC$qMX97u;hKYT1uC zqN%4xRyd;E`dNrAIn!9?(Il-?=|$`iqsX$5RkAd_aDAFX!bxS&UR+bzj!~@5%F(xS zkz=RH+7(nC&5T-X z?%*xFU`QtJf?+J3Tu>n%@kur^grbCgSl#7ctgP?s@7+J#Swmfi1{B9!3TxZj+k3~D zFDF9~bVP7% z_AKf8>l-L*2jO%Dy$zl#S750ejv>w9?@`JG{$>jpft&9KUBTIYTkcr;F;ng()*Usq ze{gK17UG8$B+G+6Em>6-i6Uhzz|T@AxDdJZ3z265Q#8jO(-5GyA>~>XB&$?@YXl?x z?X6uPAZk0l65NHtXlU{Lo0>Ml?-1x_^RSo@BfyXH{61t^@r~Xovn-zTDU;lx^emek zpnjcxA>9@9je*d^oC2AX#|}_Ml@KKDyn-TCgld5*MLI$xg{tIYr5Eba4H=OGWrk#` zEo>pFDsBPRMW$kSMrMp^MA=6EO z&^#~wNgdsc56N;$X2v(*?lTr4t%#H3CR3Oi*xA+G(rnMC7Myk*4mp!SNH`AedM=;B z*~`wV(oZ7?+&C{v|2=E#Mumu(`d}IG`r>=Ps*qH?-{i@U8 zOLM=d=>S20==%Ln@M2uI)Pfr)tV)BX3h=a1LnfQFp?)=Bb>k>W?KC_0(QCSLAUH6B zVN7v)KHO#M-Vi{9(}D#ncHGZ)hk08O2hBccT6 zjI)Abiphr&x*r$ZBm~mqIjM$FpDQcs&x{X+u>qBd6RU+=poF|p4f|xmKcaaR1^3~z8SaUJcF+F{9 z{N%_9KYBbog6R~F4kE2A4h1COZKKANWBe`gG%nqXjbzF12j@b;klUy3Y!aNJ<8N)C z!?YMa9Jj?qdlu6`eRw_%U27Mw&D9hXa%PK8B;Ncxc$JF(NHO&Hs2ZTcXN{O^(nuG_ zvuZfP;f|?!Le|o9Xok=>Tn>w8E6K9F)_9qe*gaUjJ){@i3vJmX0h=XfeVwf)+c8`j zW@!ST#efkTsQk#Ad{ycmh6Bg=;*0X$Gdl=qoR+RX0QN%TrF|eVlm1psPrX<^#}90) zwCx;+2)HVBP}hD{1#(s$)f-Kx53U36;{<{l2|u+c<)L8f`Kgh7au#P=iVJ+K!c*%u zXhYs7?m3O>N@aOGh4V-VQ1=|6Z>1i(kL@Hl0R%DRIx7%w8d@mvvSv;0LtZjvi)@M` zi;HW;dXHEO3A`}dK1VAwdEz-pCClf`JYBLHAIg&&P^GNs5=I7<$(td~kU2=w*Z}f` z?sjrg50yDCUEQtPwC$GdFvVq9KN!dnOA`7QNGqfVJel*~-Qm(5tFWsEXeQLa@UJ1J zWOQ*W*}DU5bDA-3sYA}WYWDKMZpJ2ZY8)%%N8uj~kR(Kya@nLRY%GD=rzJ9h1S|@x zpr*z&v_{))oOP#ZXT9D<&cCo>czDFVXCzmICjKcAlzs33%m{ADbiVZrT`J30ZgiZx zyC5c&I!&ddsrYL z$E(ULSLY|Q+6|wloaiCw1XIV(_M^loC&EE{{9^f^4`!O-0I_4*vAgC>Mkql)%X?H)t6TjvgQF zOYzFGA*%wdk;F!7WvYst3t1zt=%+P)${KoQilWbG2pqUJ4fex5NCArGq5c^!g^!JZ zD}gKSVBQG4%G95gpDS5n%tWS`kBR#gUzjd=X|mRMk??!col?+XPQjv8Au%nw0Bnks zqXa2K*_W&iEpf>{&vJ$h#8? zqq)Cas)`bBbY`~W8)hx9>Z3~`IKxChbmluth&bPfh0+fhIV7zF+~~U_&`VIwS8z`r z)Xqb}m8tMKvjsvY6IQz#Z``=kpf|Zkv8xzbO3PeZTQ|}EfT;#(Il$dLQx!#ZoE$qj zej=TEftwzHerv>N>yCCe zO<6=?Fglt$R;0dx4prF1N#CzSvAMMH3#0;DjD4+yj|BE4bIyg(h9!V-gS|ko0~xhV zBhXo;Ujv=TCB+{fS#V>*oiJpGGh~Q2tBA=D_;pE92n3!bB2fQHzQKG%!04e3V93u$ znHdb>=BMDP4k+OOTLe)$AFmf&6a2;oG#rO&&LXZ@ee%3OIp06jvd=hQ3*!P8hHoPx zk?o^x34RRz)uZ7Kv(kmie8(ca(DU_889b!BZkNx}x=y_@t8+{k5Lx2^Lz2V;5$p<> zG48Z5@SSQ($UI3W-^_$)W_js(No2-u3lZrR1?>D^`kbC-gTT#G>d)dT`7Wqvy2`-B z?7-}pJ(-c>kk9wJfbQqCoN)F;Kevgw7V~uBCD-EQ2*>7q5ITI7CSqdwuIXL@SMoQ6 zEAEu=!bcv_KF0vei84^Oo$oRC?AQqARr(^zT4n??p3VLHk3W6#a2rP`5_^Se7OBso zG1FWeLS$t0*JiGiigKy*0HB2IlRjm{lksaQCn)Ng?ldyU3vkupAEIu{De#VL#aojE zm7zd5^g2^@W&=Y5#n8XX`D+PRs&sP&8wN*h=or0ftKDIbj5G0E#Ipe~!K1?|9LgxI znkl7>+SEW~HWXQ}Kqq|~7Tt;S9XFEca7MX`6YzQX4N544jw)MzPR2RmRvDGkj|qUAMK4*q8$oMENnP>vI-bI%3n0X`c zu%n{(Z-pMYy&g8MOxEgxM(FQW%71JJ?aMnh$s^rnasw&ml*j|1dov6K8X$WWo;&~% z-{{oR?!gwRFou3@g(!E;!cDzDCWTJ@wXklhPeu)@h$JPFgQn9<)JgA6COW&-jDubC zy5p0^JciQH)pl>5VM?kPXaNnsM*;y8a zc~vci_;U1N5SgC_i6F{UK?!J)TuxS|5eJo5H_%Urs%wQW=X}DOThNh?wv>s(`V$ce zaAo6>ls%*X*g=N{2Bg}c#NY-E4U7qgz9LBlTS#lfNGFv8P~OMSr!v;ves}TuwJYb& zU2hS=d7AtM1!rwd2q?LLff93D@udmd6Zf395IUpGL*%%%zP#?*dWp<~d1z7r1sYo*`hth?GF{t5$Qhn{rdX+yIN&}gWOjFS9;Le-&Waw8Uxwkevnd-5 z6=OZ_xC1Z7Eba51;;cXuZ^Me5niNt(kYXQ%F=e!`$b?Ve*`{ONxJ&F%F|m*3ADEiM zFXE9*j2i}cy4ku1j2b``43iYvCl|dCBuMAVepsj#*7ZEabeC0~OT{8A?(isidDXZY zpJI4g?C#bK6QVFwC?1>XUh5f;Qybtm17kD}X33-T%i1eRrW0A+ zD0ZK-0V6CAcF0Il$L6WAW}dcOGY3&5{s+QK3mJmbob@*^I8>8h!mTD`yEg_gEAza-Yp2U^yyq62bSv0)PlkqpDijM zFCi_hjpv527y&LRID?l%UJnn?6id&=F%vd|PIxGH){7Js^CWTnY&tX>d$12)NlD7) z->cOUO@b1FS&YC_6EnGOj9`(3)g-eB-<7B4{Gd$rX=}Q3_8O-kQ^;zEmkL!ft(|ngTqT+ z=GCNH48fIEAD)GeGZkOB(o4TI?OE10DIfP9(l#G$BftD#7fy92<@vD03VB$(-V9&Ih;wy)CYFulHS0wj4|nIMvK;r{vHGxa2hLPR6Y43e zne!B>4(VL+tCWZT8=nyy7miH9w(y|gj(v&k?YA0a}cK#WV2_(gve5r^mvA3p9oWnx1c?tGG{~ zDdNvVsI!v_bU9>j!4nAv)00bE$sw_UtE&Kr;<;NxD&^ByVP$j%xM$pnJDbwEdC#^^ zU%ztpgSX#&%A?&@xAce+X$UUfQ8F_@^aQOk$>2DS0<4pOV^SrI)3 zEKq9ae`U)eLrg$`nfzN}OlvC_VAcWXn8a}=V}~Fy-Dj!maOg5Vt7JfM^HUe~%{gpr zZdr2*2{BvUbN-9~1sz@?9HBY;jIXC($d(L#&cme9hL_mPtD>pg@KwUP;?R#df+G5+2t0(K068qViWZT_kmiN7$w$`=|8ZDXq z*{H2#c(J}aHT?lt=Uv9V`BsO#bGt%*(^cMBm-YMHDZ`xx0bM813EA}3(o>|z}(4MbcpvmD|;jvT4DhUnpGF3w||I2r0%p1xc-QlX;y(3VhSM(dr+XFfi4`rO4!m(QNPa_#z+(;t3( z>e7{#UeKBuL6FcGZ=i<1yn3>Fax=5%k>2-yB`=9{(NEjf(P73K7}S}$O9t^M(sr8Q ziEJXbp{?y26b_wBf^MK)NP79eSbBTtSNw`#+6&Qf7vAk+{A2C`gh7XX`1t7;1Zg!j zZO>H945t)T28{_|vKU*HV66DwGH~SC#Sus zv9YG10X!U1Rp+6 z_Q(%{@7_?>5Y1AOSZtp`5&&hQokO9vDkl3KOj_P_YOZt>m6Gr*|1GTNX)@tQWGS&H zJQ@ATSO(FtY+n-1F4utFnVGuRd-vKW@4ozlm%jhv4_5uj6XwUi z^m8mpS3%%8O61X=Ub>v+c_zuVDuYZ->D{2-$mKBl3v;aE=laP=0n0RNab;{$Nlf3q zAL);T6`3DMkUYy`PpPO$zmn1@9HK7Om_4PBG#y}U*^B|M&^oNXj5~Lm0s#5~Qbc{( z;y|}_+bqS7PAhD~4S`9+1MN+>ub=h-v;pDG+h_ zY+ON1RDdllAXi8ll+=$PjG#m{pF*gzGA_B`|LJ$r!Ibicit{f~!s*}(aP(XPpyb=1 zB?uksZMk;#ou9n%`fERV_w?zrS8m<0g8J6YD;KX`f$uu+aU!z)(xQshZqoDW>UWk; z{934{;nztK-#a}v$fv~wq`nzOSq{&$w={1>&cXyNrOm|9SS%;Yz)P|r0XL=0w4oy$ zg?X&l;(dn(-%4v$ta0iXqKBV-{>^7LG$oM@H^&6R@v-V63c;%U2kJm2Dc)+~C) z*xAx?8pM)l(QtXm8s|bk1b>Pc3A3U37{CS>5Cd@H>HZl#o_v*|KmUINuog5hjm`}* zD8Ct1H3YM)>m+@V`YS24cC?4*(vMZ{=ffp$t)-Bk z_L5rOqm$;x-4xi|nlLuug5@Wf%jLINP}6hw>PK(A^V8Q}f9Io*PG7u!^XB#Iw{Bj( zaPj=L>$lqOIg}I_+!kz>jE{Ty&dLcZNb>^BJPid3&U4P(t-DBY(89Ihjs1qgwdEDhR zEJp3f11gQ9sA!54STRsEoQAh{nFvS*RO#DN-A{oRcg^*{*`mHoKGDTlM>PvH(=Bfm zrfIG$6EC}aMD|$`=+Q>~wtdV^fR|ibH zv4qaXQ1ur5HAy^>((ey@V!M1pqye3Ib zqKf+;Su5z&-E!mn*^l1(z3+bacYn9|?#}=8gIC^o_k#~lU$}O=NeQ>Lv8lDKy{)aw zwPKl(9fTy+b~AT!;^f$gz7URyoW(HwEMxIm-2pT`Og@SHWfgeM4Z{BF`78B?7YZ9O}H?+_sYa$j&ior!S6ra#ffkfPQX_#U5fi&8%=vZk^T_ZjojjIcnge-=P=Vyo7yYFQHi&&8@MAYOC zUDS?zTnTI83lETv_nJ}IJKI#Lx}Wm6kvf*-R;k9q0f%StP#`xIpCv`wI7W-X1x3$6 zKQn_jB|c@Eag6q@|DNsyF##x~%j$Mf`*ZaaL$fvdaW&qdd&KQYm8KU~H#V!M`4 z$9HN|%o>Tx=CAd2WB{bDad*Y<@_z77xv!-x)XBLD$%)2#n;UOjc=z>JUjE^aUi!V4 ze*Dw-K0b5y(#^YCBJKOV+tk`&`uQLy0|LgqATzt7WkwB7op|mvDZ>R#@j&^HjT*dT zfGj2*`OknDg!)Ak^fBlVV|-5nLD~S2ENKd|7u?#}gbb5gRd{j7Wmv)aDHyOH;m}}cJs0+13BVr}xBcolNL$N9@p9Sz15wCy& z@W0x;sY*I+Rt(kCgHo-nR39AfWPt8^ZSTR+e!a5#@4Z&cfz+IgPIpbTQDz7mN`-Nj z{(^N%9X_{};hn9`ZZ_>hyC+}^HWD;?|=C5ne&&g-DzrUDq3ecS{i{E(|;o=HgYD?SSluBf%y}~{NK77f%l9-2`4ZQ z&5%hpp-S>ud3L01Y$QaESi}fjRyd4y$2bk4Lg_f`9y}sw;HGI@D%njtAX^fp>VNY9BslEYw?wPqm3^k{HRBX-`;29K7#;%r({rI?@K+Fe0u6=3YH(rPs zIy}mFKrC=+`_POH)AbSnh~JO2Oz!V)*I0lNE9sHaj{{ioI;Hr&Gy$&95UrJk326?z z>A7KbHyIIF^!=UAd!xk1rbY*`LY-1!*xwR^JYb{;rS#Pur)A7;$29MYl&CG0Iu1b1 zr=yJ|O}060p=Uo_p;cKe9Nq%mF)L$m9aO@5?p!?e{u@8}>FYoKz2E!6_g?(Lk6(NJ^`E@`;U{O$Ubu4Y zcB4rt8uF#M?()E}94py|sVHd(F?D#Jbo9BrCA)4ADTylO(F^)E;bm|D3L-kIaBQNw zIbh1CDf31M;YRsa6Z#@{R`f(I@&T$$T!aWA;Yof}3P;!!3e1Dm(jiAjPeH$bKYL zW@N7TxmjI*_`rm0ZLOwkJft}kQnNX7iGC7{fQZZa4IK5vbf%cQ4#WehNMQ)?sePF$ zVX-=~x|6-vPk~O69``+XR5}2>d1{48%4p9aMdlJunV~X}@3e*)Oj37y+^0R5HriS- zW&@XZDJBOiXLGl|k8%Sk&Ol7L|Kb$}6i!O3V11j;hAnIy|C!MOY9{XJa-+1-MFTMCXzwGm1B;+Jg4 zv0lpPInc%9&CGvi@dRZ+MzIBAxYntQF;_?K_$;3r?T-2P7MTtc`{fn5hq7jW?_Oo_ zucHz|xMsb5r$vD7v+{< zgSG90U4)o1)6zd4$yC#5EFw)8=@(aK$Mk>q4Nexbe;%PnFpfHkSK_v0gi_M42*={roBp6Pbkm&cXKhB~@iin|YQi|);L8=IRg)3#i@!?GPc zO1Ls1!mUy-2xDMeX#`UpNuX^I*-At6z`X%kOyY(dg2z{Ku+ zKY8=5*M9Va7r+0#@4oor_h0(ak6wA@wYQ7+);>J-$+-)cuiv(h%Q|{dKxC5wBU^Or zMkAC7Q>!PQLYO>kIZ7xJ!`!%ICtH9lKl0~n)RT&nFJ?t!f1 z768sZf+-1ex2sf^Isdr*g{fgv;)k*o9LSJrmild2lWT;cYwMMj{`BZo*9^!m_W#3f zI--(_)XWF)x*@Lx^u6bxxQln!92k5X8i^9x`t+7oanlhi3-z=+Haa|)C^QL&=mexp z{X;fPy30mZ4&0lsOg+;ysqQl|s%P;yXh^BF&7+x!*+(ps?o<_m72GXU6>kAEI|GGe zy?_z)dGu0I>Tjtg`adZa;)#ou|D}X3MAu>pzygUdt?tg2I~PB4{y%;D-8Wwg6!7P3 zZ@m4^d+)yc(I;olUAS=Z%8ffVYyec&9ER_oRD)QqE)wVSg>&=egs}l{BYdu^K{53! zXd}|#NM{5^5gdo*I{X$hd$(OP9cHF~FBpB>2-Q zhNs}AD>-EjJxtqcn|lXZWRr{qlg(G92r6PR5AR@cedlnOn?~psGaG6mGczl;W5obU za9>-Q8Oe0Y=(iG&=2zDA<;K%<0ktUlH}ER|H3Jvw#;qM3sz;<*TF-9HF0WKCLQ2k{aa6oc4(s}A#Wvp^wa%XG9rH@~E^^Ko$0YCiy_rCYLzw_PizWCD1um0rq zpT1vwv~uR$#Vglu-fe-$E^VyQZ6miDITuMBi)DU!PBpvR9%oLnfVU`P)Ch@kw?$Y zXcqJu)eTD3K{)1B6B4S$Qk_X_nv`3nerqCh~P-es~*5~W#8{x<~#_$x%R_ry=~d9rWeQhhX}3cy7<;mfd~ zr9K-uWlCJb%{w=*-#B;b)aj4kfAysw{NTkGfAGT}zx?W}ul?k;*WY^Q-S^)A;1ox2 z`R1LwZI&&FWj;$*SXMcfc-n+ILmO!k?4^w&w*a7z>lm=OU3PKY!sbF4H# zv0KQQCTe0>taarSC&Eiq7P^wG7K?wE<5$G>*jxjI6!ybY2?S_PFbESAT|(Ad*`U&R z$7a2TOP_pl_RL3bzVh-bufFo?Pu_U*o%i1V@cs8cJazh$GiT0TxODAKOM6Gqyb@>m zL8K9ji~J3*v-+L&6Mq_6aA{+HcI3)wo=9WLu>nLy&1bs9z*E9|5=4{unry4jiadBBKDxwV1vP)tX17h@M?Ed9GOMGPtK+ zIsE1R;r=1?Mtn<(yZ6~Y``k(lcba?j)vI%*0pw;3+Xk44hs-rU0tv+JYlAfn()n)} zJInir5AMqcH+TbmxC)2iwIBf=U!qJXWNX%#`IJI`4c{!uhn8GyUfZ&V3g z??6QEV`bWdC&{uBdliRC2xWqpWzYy_qN0N>q2e$G0yPndz~a})Eddq=;c>m`{|o-0T=MWN1vQOf9b|uk`Ktf*F>Dq^|MRH5`+~3H@~xb z!UZJ}m&M9pyPIxay>#Ki?K{YeBIQ%?&jR2K4epqlsm#_GrHYpDB^$)?I9{wFJRLy5 z8p>{fVsaji@Wgp&>QlkhLv!=E-kAz9dU&t3C}zVdiw^gcn8E--X4{94j&_Usr&9WL zL;=qy0$6)x%934}y9Z#NK+bUpiP8mAC!!B`03d3qn3(%VcSgWZhmUlEYfC9$I&)pL$+k5%cP2m(MJf_WqPjczdn8bR zBmU3DkJdEXbua`KRw6iu`%O4_Vc`EpBKDba0#$V*N&RRt9&fZ{_QL{3y%%9+vVibx zA<(4)5j~^GmaFWP3WB%H9UL9@NlnU7oK&~R*pBuf;f7c0vt1wm^d(&I4`2GxE3g0b zjW^$XU*(iS8BAQ=ec=4@c*S-VnWx((2;jcu37N`icq+CO z(BDugG0Q`?oSEzCM2sRBo8_W_lG!QFUq+|{KkujZu#I?vdj+p@lZMn@0279JExIqU zd8A(ko03@9FWU^Nk|AsAXy^KQ+ClKoy9*V-1NcE<Bffr8@{?UQ}r?__?t^|O`5K5QVzI%kUPw^DFIf>P(>Mfo+=ft2yIk8fzV zVk9&?osBJ>Qr1-I2-TwEnyBxu44!}cmDgW;{f#%?dgp@=i;p@#{ODA1dPVR*4*}k8 zXw14bF#}lRikyo`sRw?keZHgu4JYH0g_@P>-1wjpmx4z#_BfXuDAd|s)&fc@x|scj z0YuRmk6T{+u<51dBvpne^-8`94v3d9hF{+;mdP)sN~ipU4(tHrHa@ik#pEIS`_cl+ zJk;~A?A&JoX723mX*JNAEn--b^x)`l2br_IcfVe&Z`V@MObaYRnP^I20*!LCF=L)> zRVtWKA8VD`ymBc^GLKW}fHr{`6`ITwM{xW*)w;p{_QsYjV#t2uX9DQcZQkWL%^m7& zZtd>(OJ=fm$o?VdIK)&rVSsT>C>ed`sC~c0crJ(m#PdZ8NKv;O5p4`WrZf@-H?oCj z#BawLss!n(H1WavqtR0vOH&bGmXTta7o~)}r7}lMW z9Asc&b-eZcSAOtAbpX!)r*Br?>UjH|;_b0_-anPX(6yTl#|8kR^U1lc?n%ld#q5b) zu4Pi0Vu+;p=DE|SPMy8l(Ae*hz}X~sLy;6)h9(T%@b>84%q!;t$!LV|< z%D#d>z%qIj_mT#UWNjbRjUiPC(DDX)s(kqt@_%K(6uS*UOprr_nX~6+S0MxW-1^$4 zYoCE<_&fy?TH_s2p|-hma9`fAw8343=TUS|!*6nwT(983vd$QNzIjheo5}^X!)*|1 zVH)=$33oP83%kFT53BQ#rOMZe{wj>r*WJ;njM~`J&FgozkO9DgJZIM(uD7e`SR@OP zfos-GgR+!3W^R^UYisvFf6;dUWJg~iv}msP8kAYQ7rx!2x&(ny(^32=jBxSA{16I^ zkQ9n?oZ6Bl+R6_I8@If>qp9)Em0OL}37Dp?wmTQD-K1Srux_bzE+h?Y>!?5Z`kQBS zZ6Ck=+N-a=%m=*r?)x8pRGjKMedg@h;@tA3Yu9evzMZOYQr1*4)b`k+aH~7O_Q~-R z-RdHSCrv#VALwkle&Lf(&RuJ4C8yHHrCy~xRIdM#yE$SGDfSs*k*Z8FKO!Co3OU7y zR2B+@jiL!6FY$gwS`32T3`MWQ)H+H5q&(lvg~ZFj2zW2Ie!0$b^AGOo77|>ksMzik z1~#_u?{5|L&JEMIw*F>&?_hry$D{Y;@S!~#T)gzo7)AYAWjc?_@aBr0V+LZa2zzxU<0fAsUUzVjcw zQ@qtB>-)Gk(|)cvzj5Kx zzIm{BAdA~U4%9(`EjYnYb?O<6wpg%iF(pQiY$X0EiBm{n%9qt;sM=IA(>eE^9wy+- zQpyJg`7=J09eDwi9BSs zB=#K}K>u)M=&Ac3xk`rYM?I19d5Zbk?xeZy3FIZV^MTS=5fNT7b7$2`x<8yP({iDn z;2!D%Y&C&*wMTJ)Tf-~$v|Mj!YEqkoR8vq8jIiOgM_>Pp3pn-WD?fhu<)6Iq_PZYx zA8nmF_2H?HKR$gH3w-t3Ed}eGf5xd~93T5coO@HVHjP+1^^h`~`Ab<)8l=v);|_|4 z%{On{ZibE15a2nEDcw@SVpOu}Gdk^Z{n?S$Xka_#$>tgIGTneEm)6&{ z^f(rGl}tw}!zEfvI*?Gu8}RtrnrmB*eRpSPOPjfcDI41|!Blc`%%-kn5K6QXFktM( z;>{g}r4UNKJW5(o-v+|_{u9NRo6i(dc-l6X&otQGC(j-d4sGnOLMr+20ahWfMzl@_ zWoue1`u}vuprp=LY*4qCq7TyOBl&Z`rKONGeVF;v9Wt=gzLWR(UXGg1VyS&p;>#is}E(H$s1^st^x*7^4f6FgZXo?m!95V(ZN-H!k10+t6^g=@w1r^}Ee? z8tyi>wKd=2#BW@?(P%e7V`Edp?K^(mCS|LXF_rDjw^zS>X5{dLqophFzxU3YZ@=~S zJ0F}rd*+kVXQL9odiC0k;?@|E;N50POs^dr$hl1E?U95yK6${@i36b0`j5~ z)(rP|+`WC5-(vz=J;j4a7@}V&1hVK$09KqeGtBBSewWtfV( zSzE2FP1SezEI09gVS^ZegRey$dYoi9dkJ+aj~kheUN@ZY>F1BPYsPD?^U9$;N&|=- z%|Rk(0$PwcI47K|JEu>*7e&~a3s8;fbR3{eIa4n24oc9<_^7ws(-P*u(_M% zyCMe*NXm!!;rC&UjCFYE^kZLf0p@yVSkl3>FCTBNE!Fl`ShLR-aRZS{P(ex0=zCl( zEkKo}S>QC3j_*aE2n|>~dJxkDdY@ZB5@|Rf9)dLQMEvC8s68jlBF-c`y$;|OZ!721 zzwYL-1jcW*T{w6*KfX>YvMWd13As`lTWjwXTs&h5MH z^op&m?XCZ!Ss-XjshR~?S=QCK{`0RNKl=39v$gB*zxSrH;QJq*K7Zl-xw988T)G4V z+`fHFD%g0}hV5jJDL*>#96vw2vLp^npI9w{ykXyLc`6*IMic??ZV89OaTVr7aA5fM zV)DzR-1)6sUeZ}gMzliY6L-0Fgip9*TybuHy21`|7=l~fH zc#r$%;%oKFR%d-{tJoPZepOXmSlB3PLm&g?z&aSn&odn7y0&R7QGH!FaL@`tjKQD4 z!CJLge=0g83a|(MW&3b<>*$fFBDy525JhB+>g^@B9D#`fu{6eBpXfD^*`7KT+sfbu ztyq6*8{rEPyFjA6Dg`0)^vcMZ)VdgZo(7ckfkXrR8VhQ@}w%`KWg>>Y6t zXYH#kC5Bql7l=X(5a@Jl#oUO`spf|2PSvvq6N(~|qBWIcVv_oL=$Is>Q4S|PVAt`Km61i>5Yv~|H7uS3WD|hVjF}JgBo)~{J{aF)mI2Ij~=?ltq%5MLKqD7IAztwlDl(jQObxM#UITpsV{UDE#EY?G&i`_O$`?sKl}FcXHOm;KG^3d&NDm<_XAjQFy8LUZTJnBwnOktb zGicP?jYJq!3n#`jJT;EW1>WgPjj~?!|oj?2W2k*W6;i*%foWERL z?Z0;Oc0+TEcu_Qswz&QPgUdg&WO*LdT~A4oFMem?M2{f>5)v~Qd`9^t)Mt|`hllaz z6~k@DF^=?jC@8QpX{=BR>Y}JeDZyMO86mV>fnuu9uO-dU>+v6F7N$7iFbHxpGXiL^ z+?LY)7#CRE+}mY~Jai5vGAM{my;u2GHD}9l*6VxQf&FEOM-xEzsf*jd;Oe6HoUPd^ zYn5s{@Q-BZg8JusVoCZ3kSQcW^uF^Ea;HVJ1Mkuv%`h~8QAv9#e-8(lcH|p0f zo;~%!hwp!^#&`D0CyRgh+0mh4{=4(n;_NS+J9GNfM`zBSD=rRSzIy$}t-Gbm@9Z@% zOOeaUx=j9YOFg_A(0q=5etum5EtZB{=cqWdH<8GvLxp;(9f94+fGs+osZ4i==ws^r zV2oBh&b(L}6vh+j!){e(@B7{nuSf~C1kI_smJ)7QAKxq6MQO{}SKMZh?!e~LqB`i9 zNHzuY^;%_PM)m)|;*=er&(JVDkc7`67VC>d06F-THNUp$W~XT;$^hm@h)}Bk-UL@K90nJ0 zEl>e2lD7bg1qnrxytL!*gq@7mhjC;HhV}N%9C2}Xv#Igs)eC3Omxtlj1o8o7J?Rmm1ZAi&LW1Cq<&jaJhjTrJO@BC}ZR z2iT;)`dbR0IqEJXPK?x_2DIw_MhVGvGSsY3*;)pWd1eE-h(MpmF03>PtBVcvi;;S) z&bz4vc^6z@29;#c7p@?F)-p|M=kS4?u2LV0a}UXDJ<3jHYiOJEuM7Lz2iqH!`ns7z z4!R_VWeO_DTlM-judszH;=>aID(mwoMFrx-d*5xd#DUwKMtx_u*c&?BKe&HjerCkA z#oC4M-E`waXQwOkRTm&t%jhb-+N}NI|1*t6$Yck2VBb8K zJ<`!&&b}V*yP>%AtA;z*FP;133{2;iUq83{C%^yIm(MEg*!ny4PcY=qEez1Uc&Z(FK%;JB59ID!>ZLJXYPQ%*2UrLxo& zj<=|eas{kSi+zH>(m=8NU*N+`?cUjsFl-}`Fn_Wob~HcK(|jA#-`D3DiGs8Ju0(wI zWaFg{%B(zYoYgPZ<^_jbA?A>xXAaI>UA6)w!Z~H_Q|Q!~L`%2y0Hu#gN<|Q*MD!z< zC|6se1js<8R?_$mBMVA(NXn(fFs7Zc@ei}cLnxm(dz@Wy?=N)abhR`z+`f79>cw-P zocd5*|Mc05r+U8r+2gc5Y#a`pJAaPa>(YhuXFfS|UfGWb{N^o7(mJ&0nQTrW!=Xz? z97;4eVZ>^y<>mJjxte7TmVzu?l6Gye&{^XMocB~^qCW?p%-{5*zjMxHDY=GTHQr9H z?c&Jt-0)F@PQ61hPBAYEEzY7pu~NopP^?9>Q$-00H9 z>+!TQ@gPCpUbQEWAM7L^q_(fac6POPaG0i|xEVPV+P#noAz!A4VZPXng z3lTQMBm(TFN!KzY(UtIA$0af)DF;1TOsp`2zyX9i-!F{F1Y`gNWxc&l-1^;r>0+{} zuxc((NHh~h)Bt=vp$GRuVKgA|yL;#M&1;t~om6RpMBqSn4yte;3*NRqWsaA~SdSSqFw z_Tk2JQGF^KZYPXAj4Z)N&oKdwnA#k=zf_S`0G|8GXjn>ljjOx3FtU&(*x~gJzDPr+ zRM&Jf_&@wC>IY57YP@ohQqJd4ICLizHc454yO){e7YY2I7#s@&gdbEpm|fi1et3k% z2~rUG*K565kzg8oQLjJ%|H`oo6glGEOsG0MQb^`Fe1rTHd7|&RZC;;Bpvz`mV1km* z!ZX^UN2*kEw1WfV$PD*lb<;+ck;?d=W@eEqkY%eGG3Z(_-w++CHh7Ownfo@hv4oE* zyg_omJ@_+Ls3y?a$lNh;Hv5W?2b5ilO`jXa`VwFz$Ix~M-#gD){QI9 z{gYE4N7Q|)=bK-C{nZzrK7RPc)b(qj_^w^PcviLl-1*CwZyzlGP0pU6X6Uav{f z#jsM)d-K1{$PBoRvr|nS7E+4&(X!zev#q0x5_AfP{0qTxCZv-H8+h#WIUlg!wbz^y zF-*9z)eWlE8Bng z_~F4m;Iwz^G?;(k3V46x8jAn=od!*ReZq+@%E)g^{VhIU(oVIhcW8c2;qE(YCoIe1 zh3yc3VYN&24OJ@_ePO;EJ#F&fLYEi2P>~2Q87UQSn~9hIkT!BR$baJv;&KIIIsoQM z9;jd0FeY^@cP1G=gP)+y%^(5XpqHZf!^k|T>ysKX&x4=kkXdK6MZ$*-jQjVG9v&U~ z>(j@Fd*SrN{zV!=hy|cCtKD1W?n|0Kah`~=y;iJuVh)P6-youYd+wVL55oP{_V?l9)_{PFx8;KWiW- zE!q+(!DyaMx0%mLe#O@0tiru=)ysLs%wA~5Zp9@}E5=3cr6 zihpdBkp%!*Gm4}PMFMyQMHYj9LUYvCcsu0Y)oV9z-nn)E&%XZR>Enk-j~_LiWJB1ndFSV$WYjnr~CGI|uE*Z8r=H&W`e!q;7H-pkL5i5IU)oMEC$&cjf zIBY?{URW++o;FEAR|Oq-Uuk##J)Re8Dyn^}249L9mJa>RnkB}W#44yUH&gMWT8T6d zjWz-NsF9QBpm|&;r;PgGMT0?01&DjSwUvP`!M&)$YW?8B(b0p02M-@SeYn3D#;4eP zAbk_ok%3#pAkIbKb(!&20hZUyN zV!@TX9P$Oq-SApj62*gu>EtDq9-*BD3sn&0Ko^FWXfyrw5rZa4i~;zo6jT_Y6q_zP zy~05#KGExTbu{0;e&zB-^nGz-;_A+?e)hAkK7ab;*|WBb*UGYfadX$zR~BsUGN=^` zp+8G98SzaiM&-a+fwj+7I~_JoZZLph&OGJ|VKL=m>esUiDVc~o>;g9(>fV7xG+8=( zuHVxJ)p`ExN=;Iq)GjrH5PatIcbGXMMb#S13^S(|d*QnjlivnEa&CZ>gm+;!c9@M2 z>!vxDuZ6$&5{4!gHT88-Jr@_n&^2S4PYnSY-4F zMd>K-MtG76ACYdZgF}Xx90qrs27^wT63XH(Vxe}W<4)uue3#*P67AHEqZrqu6>+BT zV1bN+Amii)X71d&apUH##@0LizyGsG4-R(g8+*%F&R;67+`oSP*6r&zZ%}z=wx0=h zRHyEK`Z+4|%TKMq+}W(}K6!eW%5QW(vYnFCNB+8|#nRssjiG>| z_ICxv-QU`P5EC0nAIQe`4y8!AV5<-Yn;P!kxbV?iKh-<^(#zWBFI+r#`SSI!h3t*a6I07@4c4+36K{c2`4~!??KAU>Z+KDm>8p&-+DDYk*X{u zLU1_G^X|R&+H0?EWHQQ^Asx38H7>PyBqscPog-$wT15&*fy39R zEziZLocg}F9OR)*PkH@%a{Lj@#?Lh(5T3;n7AZ{B$IGZiit>w zjgC%WuXI5{Sv~7fn`)Q?E~tYUM6=gM>j?s}RY;%UsICTdIa{Q7LeNtFKN{JRxC^Uo zemKb<2z#`?%ux=qD?%8?2BaoIW|&3Okj8-;rxLRM?Li8*-~0;t0&PMCT^G%E#>;RAb#y=DdzTJDlG(Cdw-c2A#kwcl^aQABh^6Q(t0h5W1eWcw#z#fR!KB;^y6b=a`#*g1%^!dF z+do|V@w)$=z`%gJA;HniKm-N(85di=|Mp^FM0^@PSOwX|T|G^-_{ky>(Q)ZSS!NC> zWf zO*D{bNhs0iBIX|i83q_AA9;y#q2WNF?;N!6J(%CKFQAlWzlZpxvBOQV^Bhhi(DLC> zHy8@IrgRSpV)8*~?7@#jdu=b!JqO5mM@x*BIf_Cgh)b?3z_kaK03WSaV801y=>MADLC&{#7%;BFrD252_*WojRe z-ETK|Y4mHEmn4p$1R;}Xypzz+C5AdPYQEI9c0YXbd~$JPeQ|E)-SnHMqmO7Mkl|F& z)K+87jkG3#H<^+;7NpYc}{y+YB`THNPh9%OBgW)fz z=pSsS^sLCUCs}RSb5hw*1A=6zUMOtf%XQ*I93h6k8ux|oCF~YsyUXDxpum%Q%a+0{ z4xS>2cMzRozDM_Xopbl7i>OI-*aNS_8XptR1kk1i~lI*Evkbps@fAc8AZ5b#Ow zx^tza^~2AGy9HSqDmii)P4*6SOEU(iYrZy|CNbP4o3#x*9DO;vvO52M^3BwXQJg5j z5kxBz+Rs>M=eZHqVeS=r_fEjgAOHA!1}FdU`+xo6$D4Q2%P=$(9-o5UUueL!t5+{w z_KUK*nSm(BL$If>wV{RKvrKv_C2HmJbJ5gbc0zMHTx9hFdE4QZFwYSjVlWs_c(BFXGcyeLDb`n{1bT{^5HG@L3jH4!AUP#ICN4HQCee=73Xcf9 zA>&BskQqBJe6le4>gDq%Pez_Lq@^UpMaQY(gD{XZz{r|(eo`U}{_(oF0YodKEJsv> za4McWrUSrF0BQ~oRgC}}nV*`6f`^J$2v>ohU*KKR9YBEpRwQSA3_m4burM<{g9(3b z7nc@di`(hO6M}{DNXFC}6}ih3LUjbSRBS2JCR}1a(UJue2yK|SPG#9pZlM}vMV8`z zuw!*o6VMtUg~WqbVv0C%_AJm24Dq8vCvr}1M&H%C1+Z)H>|>)QGYNc?^2V0h+6S*c ztZr@Z9~>SY93CAV9339)Zm%yd&b)fk-GWgpey3>;c1S0QJBf@44)pW$zj5R0Rc2<9 zPyTrE2Zkv8?u1JS9`=u!0%^MM_B)9G#R5?BLF0Re+=ALhFIlF$(THazHdVp0B4b zX3s3u2@#+!(dobkZh_*RR-Cc#qNui+8i*|(5Daq zk@mi`P>KEs8{LqWGHAmTVUeY@MPY%n*VYdpVLOPO$C`RuiwS^ILdEdS^_e<)dWAS< z)pYd{|GBa}W}JLY&Hb12%bPpTrmftQqKxN3n)9>z-< z8OcE2;{poEWJ+?9jLZvJQz*OasrXdKMTG?;;|z~y;Sa6YeAZ#Rd7HiM)9;>-KZf}q zyq}$tk{GYrkD-usG=qW?WEKhh#4oFb3Hn2pQh`f6(OHPhC`Rq;f@Qa~9#Qu3c=I@M z`VB-wZ_RT)K4Sn%}M4ewY9B1O6Ge?nR-LOXBeTZ}CxDZFeGc z__%yTRAHJKDmq3Q|$@HMsMK21mh%U^dK`my7RGcNo$3oU6 zC8x1#5mA-PWnrik%Q+Wenw?uD0!Rn6_eMsU7lBv0)n+C4Bjdv3nd3)YG|emmcP=}} z-8Mb*ngZa-$jIZOG?u4-?eyAE_E8?%?TW&@B;vDa#bv^99xSu4oWj6Vgx=o!l zqv;Ez16NSpJ>`0lRAeT8kcvizfFOE+bg=y-FDh65k2`^#(S!Gj`bWpdME?E#qvL~v zuUTM!clTgto=}nzZG~dgfSbU2`o%PVoY=t2_@)`|Ls83 zcTtf{QwIm%3yQ#tCC!?Wo}QYLoRUb@7Lx!AnPMb*2sNpxMw&O39AQl{tc$6*^$-o* zWD+~exa3$*ftQk!oa%J2%05#`!)9~7oO%LBPD8Y}K1G>^H3f=VAO(=Jk#ut@J)YcL-%MXG;ooR;v|C!gz`AhaywgI1@*-;i^2Ou2P&pu? zq}YR10W6}gEPbwVVIl_uoZ@h}1jHk~5PCQoYH^8t9`M@0AoBYKWF* zi={Q2tWZ;4^Kkm`*m6>Me7JvjaB##;I5<8%JvlXwzntu^y&A+=v7x+DgC2x#jFjSH zW22;;iGXP3mpLyHn}Bu(3uvwhUQo956nKH~XkCVMk1IJU!0*b%i$8q#-Id$SSkriq zOG->gO9grHzGKS3Cr3gi21mrGHiyII7Q`l{wYdoT%!bquZSKh8D(9)yQMWlOzj71(VZJM#qlLo^PISwvPu2y{*G&Eso|yhvXlSyQQ_J4X+5kvl5o6 zH)!tfZEvN;+uhk=X)ght&`qMn-%(Rq-aom$cYI=;Iza&^17~L^$44h8T!GW0gTt+* zr#;nWm5q2)A#ios?Y5No@UYOk_YhA61`q{|pbn&Sm}fC?la!j4ZnGvbL6;IIYA{+M z+?|tYix0YX>7wP*nqLrd8*U57nW9q#7w|yv$RflF3yp|n^b*;Ad^G*r7zQ~>(oWna z(+KZLDaqC}f+(_Fo>$&9o_Sz^umI1{1+T+n0ZKD&=HbgF8!4k!yEFsYc=I!p==4s` zzIyR&92#i6*OQtC=(7SI_2u|lGA`k(QDSNj7vqbiawkzkh&*667_?p6x6n7=Cx?fU z;AsjjEcH3af~eyJ1xW|JWB{}E%Skiu2SBJbBQT?L;q57BCrLRt!;&0OHjVRG$nq$j6MrZW$^fOjH*O)&g-NFu+tw z5F^xH*N7N&L{}TlsByXnXo@mQL@9pjG?H`f^ z4gvrB`v>|BSn$GO{pPE_iY$sFI5?`6^t2>ezyjtR7s4qcrV+WM-Rd;Q{q>*`Ls{u2 zxwX@oW{nLCHiF#Ov0XHNJN(bT`sZK&7SpvqUJ3}2@FFY}X$JrJ-@N5_UH&_FXxQ^7 zVWGkIqT;v&ijXNra#IpuoymM2*<{q&qHs-;C^3QYUL^#Z;m8335+pR}V>41;G80A! zuz7K`$E1KTk<$EbeB=S*Mdq6Flb{Mn0R(+9g@cQ!X$TE2(`5TRc${UnbmkD1^>EBa_Pdx-sJZxTtndP|I)fOc`SB7%C6 zCdDH@3s>RH&y+B}v-{iDyesXN%ET2D?rQ=zRA2#q$@#&xas)dHI&cGCrfZ zx2>r7%_i{w^!P|ee_$Nt>HPWq@yYSY8Cf99yaAHcL5=0nWwTk5$Dvx{$@(N(4uz}D zS};cCMRgK!vk`$|zToE3cBRJ0#6(5iApv~Z6dukIl}ro^GQk8$iHb}cghZ<$4WkL!#nC?>?Je|Xl58~ONE=3CFQaS?ijK2~ z1NurV681HEsnNTyo(R~AQ09RNq$A{;syPS@;rzQhY0J_k=N?veJbV58>2sYt1OczL zg-%3nd;4HlY01mgy~E@4zdduD8=p_l&rVOz&dxvo^!d~0b3*^x3!Z$q2K8Y<*^V>{ za?Uao1Q@}PELC};)oBNe=M-S#PJ@`jj;NQ-1dhw@NQGjFiVVPQ@%O*`-LHQ0`#=8a z$7|QG2Sz6v3Hd1V5MppKLhc6L2{7&rg=*wVUmZ1WbOO(Hyw#r08H3joB*sU_Po&nV ztLCTrH&TDnNT1G}3u)&uRb|Lwa$!X4+q1~tc=7P6(mNKXUOj&{Hui9QyvCvEkz(a9 zfoBN|tnSUAwZP5c-ZIC|4Win_w3K)!uTrixnv-1L49wuWz|WsApn!l76iNVJ93;xQPqg4` z3Lx@0TPirhx<+7l+QEF?YS^tS z#t*JbfBM$=EbrCBN%8{B8%` zB`-kBBqgH8<+M|hxh|QB$w+qis3kE6fS3>$4+M?p4>E}1Nkjo7y^4##>4WH~LDc&t zEu?0!2PKmyxxiWR^wksTfOOJS?0F;hOhje*g+tKR)HWgu&?!&}(%zvZV-){iTt5$` ziOXdd)=38IP;(|FpgoF%q6!VU7YufWuMP=`h^y}D?iqYK)?5x9tE6uV5;$!f06-L| zd_^nZbg5t(9F&ru~bW11-PFyP1Oes6#nD)n)tS) zzEHwBFD@wY)=kX3el|Wb{9y3mU=Gz6ubg5w-Dx4M5C4`Ckbfue_HF;bknosjgc*G8UaO5z zqf&tHVbk{m<$Z0}P_=*U57NO0yTJn+&SAPYWOqBn@aS+M3n}!+shkLw?>0N^l0rQ?10H+4J~(p9Kf;Fmswp3Hv-$o>zA3)RK6yN3 z*!|H}P|w0BYDT7(OOJy6c%*i9vE<-`9XCc;$R>zXNd%aL@Vj@yjLo`CZ{AK&x0Q0SFrCd_MJOmpNDQ&iE|8R-KEIyr8mxub)54$tuh!*&EtSPC z#{P`}`j$EzGUO1$m88K;-_R7F5UR*nn7Y)~wpB}Dk8K6{L$fq$b)-v6F!24w%b2cRy?*V+ zmCG~{{QZdnq<{b;@XNhWCU2GJu@xr!ClkwKVj^hQNwdlS3GgQY6lfO+B|si0D-Zzy zCX?Vl8u*85lnnrpo&Y-$85`@HNCL>?IB-RyCKR$~K^S_k(%dY!?2LE>ID2Y0NF)45 zSPz}fT}|RUq{M^=M@xBzW+E=hictpd3=~;HbVNwd-8Z* z#Sy78KoA8*nQkg=!!ye~H(LO+IA{*b=FDQ~DjRl^30TZOSo*-OFdau)Qg{%fcmB7o zU%B!lo=Y@~e!TqSRc12$0|ForZr`{W5Ex|KD+@-G9UMYDjD`f@I7Hv28x(UT1OPq! zV(SF>3EmWiohsZE6!>qjJ|1rnK!^wg+o=T!@zzvcp@2zwuKVGKiI?MJ!>DB+b!4&8 zT1AABS_?w6qar|8#$pTob(y^oe@Ve5mLj&JGE?A&%3vTo#ytN~(W;vJA<6NE(CH`s za^7Ay?7RXu4PDB1b^@4WACCaJ%$C9kTBH$Pf$ktCHYtgRCP5nIuo&Pj=WqB=$M-S7 zi<+~6MmTYr9obwZIhLUhkkpcp-Q3nvt@CG{RXHzF zENpzR@@quv(*Z9fo{}YW?80D`82;Plv?7RQ7l0Ph{7iQ*|}E6tmqpOJy81D91aRSaI| zWYgqJPfa8O_+7j5qdd32``vH<<+uO(Emz?3^&7Wt8~zVz7+k%63s&$>K!E?P+gyZu zVRGPAbB2Q@!CNWz6BA>T5}D=@t(0Q5q&5PmLDW_Pv)Q_X52W^>g2!G00Ov_Jtj#Ih z4QyOx&O$bNv@}KY_bEcjvA%pfsvr~7jFdhu6U>jM{c`ig&ZrMSe}(BIris;#75j^$ zM;ro}INv1S1Rsz%jo-RD@G}D|L=8;f?yx@~Bt8X`3BIh6`U`POsh;r?!c;J8C!=EF zFXFANP&3OEdTQ`lM1t6uut0w@U%;)1vi>&kU1O~~5em)bqT>e$WY-EhQBDjA2<<`E zj6hJSEs2*pH#W;-1BM$3ZC1RuSdmbPT@ncoX(d4k8UV_mUi2>nB0B+$W|r*23t>Y; z%~H$P0}7T_&26n4E05MU*48#QHn%pp4HN=LyE6|4hR0sKdNsAQy>qa;v$MZ7+f?kb z#fRMsk4dmOyxNCoWWbhyu+e8Bp2;F>=7~Uu1HuoEaBO7O)23%2o*SYmyt}t=69g_{ zwtD&UrOTIpylhUg z{DwFR^+WXM=jSGn10V-gY&jImy@s=omGw?8DQmyuQyA-WITMN}Up~6u-!GG_-g5lP z%u`Y^$%RgD=jFN_2$3>zA>!bP?{NRZ*bsq8-ZMZK_{YEirTeJWFWVgWb<`I`JZlvB zJR~AAQ5n$%s{BGXf$|cHnfM8mxGN3P@VW0dzq4z~tx(APLnXUlwM_(q7x)sRHG#kmpTLilhCSf`|n*87rP@>&h~j zhAKyX(tsKf9H3u~ofIzZ^+wA)kp%ToE!(~;ShTC6p|om#duwZZn;ftQ^RvGP`Nt(V z*`I&dKloty$`v)g5gh$(REoELuVkF%eR`+BU!t@xqtbw|a^BfHwOSMtXf|;{f zjS472btBS?P=#_UEhUbPn-Q=)0e;v0=oOOh{XhnOw{GFN8*W56LXF@yU}02LSVTxj zB*Za4lM6s+hxg&I^mZn*X4IBuSTmFec=If&+g4~k9DjiO>Gb$LZYUd`VHu69^-oVR zjpIj2;&G)^&Ck4gKKhs(aDSwMKjNr}8~jh9LsbAax9S68LMQzPw5w5yK1&>&TrWi> zctrxG`#4boHhgIUc%h-KC8$E{po?HkA|2o+piD8zTcA>fyB`xBa4QgF@x*utO@&Q` zD{vztsuZ&^QK5J4-UR{Q2reA#W$3oAt5j0g>^#-`h(K~#orUUG`a+)AO2Q7}nrcco z?cqT;mT6Chh(V(d`%=cxO-E}h6J*7u4MuAbTn_?bn823KA!@;nX2TSPGi+8}uU#U1 zRSJiain*P=y~87D|KqdsPoK_=^UTxp^Uo(sqYsUdw$bPBHu*3d;ivtDw(=}bYAib1 zWZrO0zX+bB+-y`66Ozy&&;|i(qX)tp&^4eM76lyTOh>`T+#?!L4T$*Ry&?+`rU|KOu)j=p0Nsp~_ap?80W>gx5CH8yG_J02z55a(%$#ytm&%nE=Kt~eL@dGAggDp6Rxw@Seo* zFu*sty+(FD{##}vOL`w}fhHT!;Hsa1fE6nO=Eot4>sh>AQ#!o8wP~gSV<&ree-9vV z03T!=d!PsR_KiI^F!9(pEZsjndRLpuO6_|Qam>(V(STy(lY_?V49JA<$6`FQ(M<%# zdq)9J9tpIq^Az+_)Ypll)sI*w1P9$9bOi7}cLUhS7a0>z+c1pL%&>4H3Lh4pc4|S} z1t)JE;EAWz;>hD@(~<=bi0xL3Egz7H_<%e>Qn033>?3@p>OLGm)D)J493GAU;BjUR zFHXG}fAVRw+^}BI3;9i0aRPLf6)x)p#&}4e903i`}nShv1MM4AsY4S00 zln1cn#Fr{DHZn3aB*@?Ia!OM#CJ%jmop~ys0E)2RthnL$6XW5;99bSTV$9J1BGn*9Q;3?#C|yU61XTz-K##&y2Q_Mxg@KZq zqK8|i4(#;g==emf0Lp;#^PhhDe7*@iu(7$lck=1yPjm;)e>$D6qoNJD7aB`^W{njQ zzrdj2We5OtQi#?Ov!BcyBYO%4Tx21UIA78qLUBqwdXLDcP%zJ}Yd3COy9N-rejOx$ zlqldHZGcFg0#E{GOu?ZMkuhK!R4Tn%oX7B@WLAX7mX%$;Zh%0!*Oe3#5*Z$JMUv=mzP;*yD>N!Jz_`;A z5E!0HO+uwlJBeZ<8!HSj1n&eF5{LxDXk@qX;>eT8oGGB$Od)D8JS^D1vP-oE^bt9Ve251|W<*Potk}*;=LV89 zTeJCDhWib?QX%%4U`Z7SFvb+I<)vPbS#b2o;^xxg64d|F%JTBk((2ke2hXk8+dJ6X z`^W(G;NZvtS-{wx+}xTkMd_jLz8pP^Rq#Wk$+w?JC|CMyuN~-RdT$eWiL7p~ESeb2 zpq`Er?-3eo+;s=vWh^VSD+OF1Pq zWjj|3?9O8kD&z{#4yZ|{T%b&ZGJ-536~Pf3r2I=%kw^DzuE zP&s!EmEje@7>EPL;6i#4ND-s}d;tc~xx^HE0fFEApp81FuFP#U)vr-x-Cbi!$$> z;Ve)~D&3k)dmtk8PHn%@^M7`C_BJJkMI|6)Vvqq}Z6*Yy_(E{NURx8z~B`DppdO=gi5nUB>-jE z<-DcEKj1LdaPfyGT(MJ9)9j0(MHn3u&3s4<|A(vy9S}#yVs0a@ia(E}*0eHqtf~pP z&cFG+*YFtb60Vmb9+4DbFQp#$!AQ?UFJjh=hP6=CklE@}IJ5zUGFt>vGK+ZU^0FK) z^HXo|1Q>br@Zo43PYih(?_&1_PX&(^95C@3;~I_%A*tR{q~IXJ0v0ZFn!>w0%cOy9 zpTW{1e1Abi0oVMlUA@eD@ms;vIiX=R_adP|%?E?+2|9J4SaE?Rub2mf{~|4z#k`WP=7>P(M`Ptqg=mR267EX|ga(R9EJJzN~AN_I%-RKK>{ z-E1Ks|MD8iXSz_)0@S84+n1A0X z5h%h!BjcP&0CW^yya#|Y-XsSQphS4r3AqTkQ&U1+U&ET4N=Ojoq(y*%>jN-V0$o0c z0nVQsK#uf!a9Afo+M4Izzn!2dH1_1l^JaP*(EqfLoEJ#nGz~30`Y>zgzS-5J4DeM4 zl$VKURo9s|QKm9LhR>JKJJ)aA2@MIne&sq{&71zW0t0VezU+TDJeDpNgik6GI5ifj zHz}B49{Dl8_7{mRTr&Ycm;m*=5GvwWnhF?5M^x5K0pFOqzx90CD|1*JD+}`3l3C=-8ZCUFFX*cxX|hzdn_)3(}lce%=(v?@w@`Jvk8`@y4_(9NYxVAx7{3#AW& zcY|1qLr5AEe<7h8W|Db-WCCs;2wqx7b|HfiFDJ&H+#ixja#wGG6E+ZFVX^mj z-PQDmNSB~f+90T5N<-P`(w-A*DgK3ErRhHyXxU`}JUH!AN*BEH z>H5%_XXZ>s9ZVQ_L@J{e$YWD`%q>EfcXE4o%h)W04cgjXTiH~>x3`Zb=&bI;B}=mUgq&*wrRkRt5rbM1`#?8l(YO(exaJu{ z23!`S5dJ|yxAgRf#jt|_iveC9nFThNgJxMcAc=W%2_(!Vkw5|c1O^_Jq92X)MkL~i zvGz7BT?dDH2b&;|lJs>nE`dQ!Ar|C+twB#rmzl*@ss{-G3@Vs~iE;ta7rRY>D&1JE zU89Snz6?9-s@W*ZZ6Kc!G_*yT)*#{~WP&wyMOleKH?G~xLW`}L`1D{8I*7i& z+3A_(y!Z5%?WUAeM`p?7@xOh77&RD%H=K0CBYXp(ynuBT4@8$&%#9Q^7L@#D^&2Dd!0YQN#|y2}FmR zYK}vP&`=nJbQU&KyT+7hI9&#uYZ1YpT9Xh8-h(wrOtK&r7i_d-G&5hy&B3X=xU8UX za@Onxa05ohYEXv){hV%#t508;EfJFbDEMRKivtZRPy%fg20k7A_j_sqT*Wl}tF!*b z>GOX_!0IMT3meYh>D>UVB~h@0SZGPv)01Pu!=vExc?BZ*BBBz&<5DY0Y=lNemq5y7 zZmm9!Lq4^QBO|Gx7l{F*AqG(6h#iaGT5l z+6%Cddh?B<6&h5mD`u|(u?(D8#*f2JAz(jqYfPyL0TyRzj@wn#HT?Yb^wR3`;=;n> z!iPEJKP#)ttI$B3TU(o(yN6GczYmCxbM>$9tQlL&+dJzsV~8nxIvQ0Lsr65Jq46Gi zV5meG$e=4^C0~&jM5|MMX&V&qKNC4`N?Zi<-kq6nX@M#0CRbMiJq!3?5U{5kp8GGOkHTuBwV9m2ymiZ znn6d3_*2yTc4BN8x5U2g?!H!lAAeh1y|RkgMIuUMdi9C)088@mu7OI;NKUqAVq%Gy zpb~ek%=D0}e{H!{EX80oqk(>w-6sGCPvB=*b577PMg)Uq9#>!d5;M*7Dy{Pp2CRYHb+ zP|1MDN;BsaJyw8$lXaG2f5miVnF0lH%U-zNG zM!??Y^1SnS%N(ew!BUw^N1Ev%IjCz(%VR(y%mwm6rUuP{Avy_^FPuN^AM^bA*{W+w zvQpzBBCOdh_g}nS+&Mmmq(fUyVV`PrPl%fY$=sB&TqGF@6# z*&GY!&vt_<3~cJE^HJz#qpyTAtw-CR>yGyKz3m&|Gwe)CkT!)rbTsi_T(!P6#2?)X z4I)OyS;goiFpVB^4}Bl9b+k%`tx(`KE0+f)k9FvbRaqPr09f&v?gpmInws$ri4VZ9 zGSrByL|Z9^HG^s@8;Em=5wmDU8F`lc%_0O>GSXL{8rLj-LhTb%fIu~Xa)|k7(_oj) zx#YECC|`4cgw}s$HI9w#{ZF2~m|BF~TYCR~dSQ8a@x$Ev4<8m5iT%df7dXF-we_{t zjh?ic=fl+{Q`=kW1FS<5Z?CT{e_UWx{N0Jc8s1c0cB5RW~a@GjfQ??t<1L(sr&TpOUqd z1VD9T;m`>IU}=Ehg^a&=V0!7Z>H26~@c&bcnUxotXyg^dsS^2e%3i*CIx;*s(2vZf zzn-QA`GaUAnm9}NoUcpjH(kD)$cjb8{K%l&VF~G8ZU2Gaa;HS8OY3{Z_o7daW)w~4 zk*PyHrgUnooG)Z;1|yAXPZ0S@E7U(mcm^BEm^7ruFM?mBq>u&+#}A@o20rB?GDs0N zZ*xyqPj4SuJy(<^!ifGtauLx*fKreE^pFTKJ%ZvR&@w6yzRwX=tyO8v8q&^3Elhb( zLEpa)im0lRg+n?OTqClJ;d(HYsKvS-KY{Af+ScC3&tFZiGxxDJGdsJqxxKlzvbegs zvA#*`hvLq$ZyXd72u?@5EpMJSR!;999_(562X?l0ceXdySC;0dC&qiLi;5b1dfH{^ zAmsrUjjpCnA1acm1R5x%tg0pyA!yMY4zi~xB+xN&02;&DNI!(19yBj7mqT?5SwM}( z<}hbgSyvfNMILu@USWme2fJgdRFClypbY;Kz7-A>gts`3VVc$U0d4TK@cw7uJj}>s zAvv+Epz6)c+Mec%;H2JD?t%(ub=J!&Yg@XSt8k~vv&V(p2~V(R zY8_Q&d46VUG*gN3UMD1hN+}9XRJS^AhKbV!CV&^D4j{jc>W9= ztFd>(UBt7mRhcvn;lWUrQg;}hGPb7MZQhoF{vo0M2iXYD!npy3+C|CVgvR|khG1Z? ziyTvLh*3O>F$wz(fnLZ2)zBd!Xb}UwVpaCE_Xu0~Z&4f;Kv!m!q6Pu?*CHh}s#sTnkeUrL}0{Zs>Fqlh%n-t5UohtjxGRlfX5ls4|364mLkP^Nr%x=ipaa6B;ros zojWwdLL+FEB_tZj#Y8s51a{OdOjs#hWmvpYsd9I7^8_Hl2dt?wik#S%6t{8XnOjrn zg%IXW00o#7Ee1T$_G4tJZD3(-+p?Rt55c>8u(dQj^=|Q_G21gg_u<3h66F8N+B$Oq zTbui9&t|tzjwjw8pY3g~uM;LVwzqZ}4A?bx{`dXurKc^`Jul}L7r6~Do<6}ophCY>HVF!75@!pQA_ z(7`$fnLGJj;Ls!l$pHxb@wVjhVv16Yo}9PqbyKR)9E zyk1yY`7jN(dNGW@HtHfIkRk+NBuG?(Qxu}yM^JA8J6S~kqJrgJphiIIoP)1(Hb60( zZaG&({(c-=QFTwV7+B5`XRZQTUMBKYeIs?I3sHtqh(!~OEd3_HoHRFvtB7C;^bmCi zHLh=ORMg{EPc(e~;r*Le&&DAAAKmY%qc;kJ&U*SnBfF2Q$Cb#VNG+>vy+8JJusX+` zbp5;Q0WoRB5C-zA%5z+7k55jB3J>QB1o{l5XiJC*IMR~iOfMP0pQezgMLBAs8zl`c z11MCIu`@Da#?Y(q2l0%1s%TZ0iPQxM;T8}#c)U}}`uYclxS~Vt_IRYB_mD(Dg;FOO zI2F(%GWXI`E2A}q@>KKX=|v% zc83@sSNrzC4=Z1DJ?D9FyuY^aVR`RxV{6N@=OqOkADtLyUnl@hem;7)jQH!x_zsN# zePDn0K(nIU1?a)8504uvxdI;--Z5e~J~q_T#abXNsR#iSYf{nZ6qr=U>KXDPI~$BB zqb5vUzG55g=FBP4?#PTBZ3;p>op18k1(}uIjZ}Gv?F-9kmOv~L%9P-^0%QfQlacQt z7#KMoDGpo)tsYByuClQ3By5Qav=kU=6l!ow$B`}Rm=v8ci?*h zn8YvLvx^_5XXigG zd|dpvW~`R07reYOH@|(dJ-;-!eR#IFwXwZzY)vpCw4-D|5ZPaS++5c?^>G6M2`s|u z%JLF4()1*bgahr(t@xDVY^atc3>7h)2M?A-oo^<0%X#f;7bDW3XvYUh!7VHILe3YC zzO1$kzKima5Flj+7YPl6;3ffqZmiiv5j6x7P@9Q4erOz?C?p4b8`1^^2%LBk&bpN) zZBtWYl(pi*3A zR|A36j|90$Y?k54D?#YNgkU*#vO6FEb}*n5gbfn-*JyG}J=H>OefPuB*HhC|WBo&; zlN*QhcgY2W``z8GrI~kc-z}}JE-WptuAvE$7WCkNKR?=B+u0b!74XRipyJM+u``J- zfSdp;M4@o7#uezF**G{rI<&*ZSYKUQn45V&@qFY#cY6oEckFN`6$lXEa$8QAH@jcZ zKHf@-IvlR(t?^cnyNP356z+*pnn6iZ+f>Cml`nea!8BxK3|~<~P<3#qa1LfG1r?MQ zO{svOp64G(HSAN^J+EO6S*-HT@ElZh&(6Jm{Q?*`{OJDUR(KJb0+{jQ8Y*BZD>71M z^T5LqIF2e${GFRuZUrYgbBh})DBAeiHUE&l<9%a&UcfU6zq)DNFcpW^RNWk6 zJIZmW3ANwNvxW|U?Q+o-AtRVvK^iM&Y0Vqy59sgds_+OPQgJv^Z0$n>gE%|)^f%ZU zp9t^=3ZgHK(`YSjlf09zgVg zEm=cr9j&8^%C@1A=dUM;1A|W{w~o(GPmhh0$C?7%TU+|DFu$?C4+_}d=XRW*fBO9S z)917E)2)rY?H6qwJ=1%rfn*SIaCmY=vY=u7^WnkP$C1{ihaa{{C{z<%fn}}$ip%Gt z4~9CiO<)Fvn$$#vRIbJ9NKk}h)+(_IaDc}!i>3*07;7dtZ|#qV`qFZxsz%zE|K8?KQQprA1&ReI27uWflhgqU3qX>Il>Vy9yJkO3y`Orcd+_90 zS2g)W0xv{0qFG5?a27?n1OS@8hTPQ9z#F&k#H4yqVMB0dWjTo9kkpiVzR|uBKK0@z z0IsMU6qq?5j|X84IF`~DckKM*kr2HA^nmd(%u^P#L% zK*Cg7cA>lvttYHM5um0~&wc|&wp~L{U(YU~^dyu+{LYQ_bv~Zo)}wpj;KSbD_S)>L zmyaBLduPghsO8ce;!gk`X+15*NVMi?|Ag@EG&k1?vxgTVS8_!zD; zXH{HK)zQ&dg8<$ruZQ@=2S@5fo_n5WeuIVuJq*~gApZijBgbZzi5!OrBHj*4qWm?@ zOz7MR0j!EHc|7}ioc3S8vOt$AW-YHOE92P~?+;8ZW6xTBeW5EpAtEryTG>!pSPfr_ z3mQe3&686Cahl11pVimORA3gRJOyfgKJyPP*bnhg!eE&sMPU){ZCEsLB$Rq0y40d{ zcmQ(_rY8V@kOU5ImXRy(Gvm zcIU?jd!H>Ip79W?*E_Pmzq3w{Z2ALAfaT@YjorDYkS6^QXG6n}M%&Y3|Mbm8`}_S( z(BJMB>M#fcMn(>egWQwjwXxdbmdRbbgp5P4;sPJ1#4z@rZf|U^F3!DInK&}o+o9b9 z?0m_=SpzZ#+7;E3W-+0~`6+n#bXcVg1FSoP->_&c3PmjgSB%Aq$>Qe$(J+R=giBfJ zg^~<#j-+sw0ytd0M1E0OHD7*i0jZd!Ew6aG^zQY`=i`r`3_pBuzriSf%qRtC z5ATFD8f-y#9Z%xM(##ZVbWo7B3iC>Kuhryxc+c$^N>nve6y!? zWQ=l}AUT>i6?6{_ z3_?`)ch<0`7&QZAkZhKs= z4qP&C-mE4DJ$MTmTiSacKb`upwgtrpe&1Pmc7JGM=a;|!`O~L!%W1{&@!`(L>FI?v zz~J7_-r@Pi`^kx^*T(anmlLn2?q{X@-?&ro@e@X2Cr6Bk$TEb9fzy-YeU#$Q8>{;k zKK=Q##q<$6c60Vz3S{G(Ys1eORh7lkEmv`uf@$)W@R7%`U7mXn4gjEy1CEttqU=W>uQS7mAS4NlZa4)GOx!)tw|f(u@vY zTNdQ6S5`j0IQ{}jpccQ_M5gRK>H<@r?& zwbl7<6i6I?L0Jtt8o7!2rukZZ2B>BTEJ&|O#5~>rGfv-&O_XvG;TLTOe?);m0SDz{ zLAV58O+&~?YhY125soGXFqPX1H)?k`b+jV;X+rCpW4OzmsOKpi=pEAzhX=6ITCmb3 zbrJDV+R*o+hE!XeJ{zLjQf85eT=2hAb#m6s5s>LIpk=Q{Lu>bw7q6!0mN$v^TgHZa z2W^M3<6U^v*g3vz9JtVP7<6_*}0kN*~P9*YeED9EsBt+G;a+?{h65y zKkY29tb_8mbb)yG4=o4vho?vLeFX(w?>D#1Y2raVG1O>c-IQsWg;Eh) z))DCB$?S~mTJnw{5C|ctZ9GJ1n?L|U)HH^p^4ezJzkW9MaFA7|13j&9q$W_tTv37v z49m+Zo4adx5A!p4pA)?`1HD?1fl+Z@K5aBijT(Bp3Ugc(7)*dTeFj9YNYpOKDV35y zALet0=?h)$P*lct5tppsA8umZPJo4$9wocB+XpK`R(Ze4wft6D#)B1`^Y@X!zidiu>R82;6C6cB8 z{{pmhkG*^|FV1I=XY%>}5!1e8?ByrTJ9e-;{cBhsM#;?DX47W=mNt&HKv?4PC5EF@?ZAcmk|;8Gm7;QExjg${aTVj0ix} zsuuHu8e~f)7~WLjYs1@z{6S1798cwEbQ*v-TC1!M=VMIf!U zM4zKi(0xDb!#9%0)DUx#__y13Uh_~>Eplrg3Zs_ z36wr@e0ybX=HuEnDPiwuv@+Xclh9fett~D5`sLqWd<^%890+q@S8w~9+e!)Rj(ED79xHZs)HflaBhHp?Js%>wDD z&){`rd)!df*wNSDQBC<;RS09F^T%8T%9@mK)aJtC(boX+LjoG5e}U}cj$lo~SKys5 z#5@x$#~IN}gfl2E%yj3mV77O8WpQ?T;`OVESFgq%!cX!!MXYwv{2V1jeR*|j516T} zI9m>#uFAgdCf-Bz5&7oH=asb#_7>$zD#GNC%V(wnzaIuNDY@zP*$Bc&I1oS*zqG}X zL?Q|UPC{Uw1P#ZDK!u9HWPLzx2z_WfNDksfvDR#NIg7YzEvk^4ODOC3T=Re0K~0Oq zLF~wPkvf--kx{8LCG(NlsXhK;(;?~v?qoJ^|SznD+K948f&#!E(8(XeT zsW>O;>?tG*wJr76w~uWp2q5pr?sI2*edFWQn>TN#-%iiJdD4;}c?(G!ZXIFK$*%mI z-bbTrbp4h-uB>la5DHX*{&>cBF$j9sm|4=l2zw;q$mXVHD~m1x;htuJv5|ql#2UY~ z?9@XvZEw;)Mq(l~@cik>$b*3nwidKB^4uAPUy6&_4vMOsIM19dD0O%LJcX^O8E$iFUv~DS_2n>2(+bv!7>VoPxfTj^b8C>7`p!eVy&yU z31AGCUHXld4eo=QEeC%)Nc(zC2NA7n-;7857voZ7j{fFg` zD{C{)TJyvGu3x{2m2+??9CT=IRnG#+oYUReKPIHZ`4Qv!*TM1m-b`~&+0ZIayU2ip z3x|&|4&)n>PxLwbj_LqlDL5$Rc$ZtS^6|s$)Ef*L1}SnFQLQeA;9=ku2|2Lm3a8(an2_Sl zYkKfxY<%qL(?0V4!!Biz#(EsPDp`Hh)7dlBgEdl9d08#?3Cb@`TnSPrU6hjAwzk$< z0t9SDNuirl$uv*DnA1l)RSutk!GU@DMn(rzvoaWCVj{TUt~hQ;I@lO-Trh!*XCP9s zq(gR6pFq-b0br>ea7V>G{R75O<=Z9M#~9RtI!-hDvTwMSJ>nm&GYN_ZwdquxK+815RH zJpK9SPb7eI6@G`idz&BT7FM?j4GXV23#0roH3$t0XEknYLPTEmz_Ktu7U3uSmD6MX zmi{VV?tFKqt*~)?pRe`l?5lkQWD%-Szg~; zYO5PMI+`dgE41p1Z_fK#QrE#Buy?W~1Dpvnq{rXm(@qU71VZr&!b452BPT$>O~E&F z0l+bog+`W(%Fh58I4!QMbj2p>8w;;W`SUv5x%5>SRUrBBt{a6ej@7rpx5nq+ zc*sOP0A9fPa^(eoDg#lc5-5mP{5Ld&qBvBsNdwba#)O*9g9sj}=}_ForZ~{AF}K2U z8fQ~qcTaB*i@Lfh;Jx@uXpzA7*&H;Z#)d~@OKMJ_=KuHO2YG@(ctDi;<>zL!)HIpL z&%)M`*Ken$-p?++P!f6jhTp9KLO^K1cV%-M({Eo6vAwwK+1j2ef8xb~lFGs1`KPr(Sx+109@x ziIMLpu~d9PY*B|>8Q848qBx5ukM{!icUpC<2bana>W}9R#0T(YZAeynVmNE+ZurM0 zp^eCr0iEI~YI}Oh#ms2y36&_OhSRHUsVm0o2ZTtlW|Ja-0Kn+5xr$rc+PH0*1xPY7 z;O}&{(rFpRE}96w&AtsjeT?LYMr8^g{0X(kscTG_PsB`#AjEI5Xs3b6yc=w81In`O z98ZQT4IPlg&1q?g=0FErG3-aGVe>Tf_YMvj1LM79ej$4}1kN95$W0KzPl2yKQWifr zPd#^nVwEU^4?-XvEi@G!LwoC5^3L>!N#f*m$jll29=C?mA&c7KO z8F@N?d@i7`I4|&ja&&t9@lknU-{PPD`q}uDMNZ(?PoK_>Gv_&D14a5ovW33;DgPXw zTsUszBJL55IQ@@{3-B>7MiG*CHPasy`%|JFT3l`DopSn?4Hd;qk7#C*GK=Y#YO_iN zy)n!{N05Fek-s#@X^jcHUuci5Lh{rqA!~Fj{aC zWRo0I8q&deu@Pe#2;2ec7luQyR>&a&Ir1$9ZCp4OeUL1*xuv23Aj11Z>hB$WH#4)i zif|J?-?I6C2z*ne6Yy#65~wBD;omHJ;2!|N-o{vC?W08%>7s6V6!{x#i|?P0znpl( zaKun`PGUG}$FP{_tG~Zh`|5evAAkL8oSQC3Wc0t;ptmRG$Uc|m0ODu1)sx*lyNxDr z^FM62?;sB~i;S<~p6?H~l(Bxpbf+S<=SJJr;ScbO-s@G6AlDSmB$vbYBc<-g{+3c0i_lM&cxpP z;(`o^_CZDB9F3ls2^%8oSLGCQXaZ1Xt_D$2MU}IZsJxinPxURGo$cMDPy3rWN(Bf? zL*D`4ejnDg)9T&YztWg)!oxW2ci{keIvgdD+eBNl@5qjP4o{dhM?&ecu=K7Hp&2ADG z$PBs{+pBXEuiwwjFD=fFx0a^G(qoDW^Z)hV{i$;Db@(^`_z(Z^PrvygJQJuutzaCL z>r3x+0C?=8l^9i)9)Nm&fQ(6gf>=*6Y9Qvt*rky~8f z+TGhf^y>BfHd8Ob?)myo-v_=AS^H;lP7fee<+_T;7S>wgB6a!%e)R(t@j3S_Mv2mr zQt9loCnG) zBBMjU`A5H|`48?e<8E_sXk1qPixsklV!O%j93Pz@&$i{2J>L5BpJ@sIhfmq)g7iJ9 zER0XibHaZ%|H(Ju19%BJPAw-cuAg!GMcLp;V30BM_083#>B))bW23{`H^}khwgq{c zhoTXP=T3}{mA{C~&Qv=tw>0UAi&d5AFwsT@-2CC<)jQF6DWajuE~$sS7j=x@83GO0 zCxS(?7o?1!FcT+FkTfJ+4V*h55d0w0nPtm0O>ImulogaUwHloTea`?nKz$NA6=H|) zknf<6Q8f+?#dHNY|iCOW_AVSWb0PMCf5dzxf&J~tzKVvKonsob zROKXJD4{jy;)L;}^68_Fk_>lxa`e66(5v5m`@68xD`PL_=BL+WHOI>%3w&IhpPzm^H8J+MzYWzQT521S!$eB)xI^X`!iVW$x<#EYa^><3P`Zutn!Vr zM$U_{Pt=FmQI!VR%=Dx%0~CXAQ@RbOiWU@q^8ZBj%{VEQOHFq#U>_UOu4cBnaN32W zb)7zl#yoxQShZ1gp9rI!|I6ldEf-qB5%$n4}$4_6=z0Y z-~IRh!#_tR{_21HpZ?ST^7sGcAO87QKPEK1-p2NUO8%%!X#ezVrzgi-_IB5d9LM_v z`fQ*+-$uVIWQQCca`Fty!DCqBY`(pj5Tq7$%o(pdlhd2qtCqvR9PxsAA)D*VA7^RE zjXxe}uLJHC@bDL8*lbC(I>RLpg+Iy7FR!U8awpvlxN`A&KyairAvw+Aq)J9001t$& zlWv!CcfLn^s;oe4JIyq2W)a(v7_-?0iIc*a9v}1%5|Em{A!9JFx39Oao#n=6eQ4y*aPAe79tb@0qX211C#cUc z=<9A{%us(&u3N#Tv4N5Cmy;jXsL`3oL1f3{z5^k1dLqUJNN;>{e7^A0%g@Am@VBHo zkTIW6<{mZmZU6FDto@neCGt|S@9(b9Q6Q{*c-d1|p2u#UsK`J4umABs{rz{T-mAa+ z#`t6Vw->M7h{){!aQ4fe`3IbjLGKq?8#tzZ4Ct*#ci%D zIVmPIIy#hMl#(@2 zn2rP|GYkLw`kKbN!D4bHYr!cJL>8img)5^u#^h^VTkoUc5l(-EM-k6fPM=me8y;(X zdbJr5mh1uJ6@FbMCfF1v_8Zz*}f)uQpi5;^s6Y9(f2QXD)1x2-GNGc!N?VQFz; zkq*k-yr#2=`uMZKsqp0UkFo0_;{NS^wi$zYzPFI(?QX;DY(1+j9$Gf@JYby|yhF*o zH2>!D{no6wy8$)-v|fByUb`cMDzPk*>!s~H*^ zp4ldi?2EKLIGn9_W^~McmGlsPjI9gXUeh&l$JicKt)XbIr&=sO5g%l!oV2xnJUf6Y zt+1~B$t+NX1c7FR&zE>~{naz$*UT5XUo~-GIspZ~XeEW}f{9AE)|EM%^~O2eZs7;w;3wDeMdIag%t22>=Wt zNoK-U1oFOgIJ|OhNfBxS3U^b9D)W=l!LDWmu&r(FosF1^iMXyPwUnXCkb{RbI`B7I z-`v#FEFbTR5;pu4wWu=%4CzCJyF#vCS(=@hXDDNBopGHl&I9ab96L|g+f^)NPpIdA ziui!=Q_JxbJn#Oirt*jXFHP^^-DJ9Dc|*UVd#(8krn~Fj$~i$5IcF%MZ7_~F=bUqr zEK9cJC`-<=Wn0bx6ALJcSmYc`aK?#D-PAMFvu3fr-%;IfUEoq+Y+3K~oUqT{`#gC; zI-g@y{I($Wp1c}9RdZOBCM4qcrcM#S^@W(=V*iNH1oh^F{hd91ecio%lr>D|nLO1vDmy`s z<=J!btpN72y4&8`0rR8I3MGz$g2`o>sq^~nTlc8%9zMKx_dZj%VKmuLa)`%7;5V0LCl2+K8fy_V$)HSz8?v`qofLstux|ufah!%gc6gt;{gh2o zIMEZ>Y}J%irW!(PRau##xQQbm)w`r~nZs7{E|=;F$d2n@MJ_Kv{>FK+A%lP-qiQYc zXk%|%v(4T%*TxAb;CkY-SA&p_C=a^M6El;V=sd*^R8^VJ*_&5Z!=HfnP)wpN2cCm6 zN|z3QO`!^89;g&P7XQ&k^eZ-8*&7;)A6wXxP#CByuN8lfSI?fDsTt_$s!dE8L3}iL1_#Nne z_2>w7|K(Ow_caF!Kt!O>zSgF6)wk6br~5nov~}ag4eQshUAJ-jE>BOKxwmat@uO;? zeaVV78#eFU7aS6)?mB&EXpA5;LFIp^SFN#~8Iqaq@W{Mj&B)li5gjT~rpWU~^QqwZ zvj6<~mf0Q!zvQK{TXyUVbF!;`8@cy*e2PCNYm6`&7$3TR;^>((Xt53s9Qg%7=Fc$k z283T~t1dU@q(u4qdIz)1IUCkY24Wf9lek)d_X4kwVR4bd(8gF`gjZ`Rr~5KiH`c=5 zb#!Db@GfcW>9*1j)U-D@R9CBnDN-jpw1D!tay4~mIqb4Du}N09P~|tw$*0y+8r1bl zol#l{LOi8@jmlieIg=)z1JOt^z}2OYfZqYlSRj5m0Vph_6jY3lg#`p~k4C|0vzy9W znCY~&cQl$Rp-r*CTk3bPYKjUooI+gOc<$63_zIYeI3UR8Q+e+8gU8O_8M=A#@=X!t zvXgm2nJl4hXM`$MHPnrNV+fZ&`;7WpE4B8#8A9$%2|@qa^ojJR=Y6iOdv~Jh+u2%A zLX`;aP>GOW@w*_(GuySdwEmIh_1sA|wOLWcQ4>;mS4l_~z>kY56J@1J%^KhVt}J7A zE<2eW#jdOqHjx%u$gg^M?C+`97+Z5_{9L{<)zofu0n{nyvZtNhn* zUMjB$`mbKkd-asqPx*}^_jXVH(MNxI`&z)6XU|@}89VbEESr;k)`B?9WZhW0xnuMC zH5>LZ%ir(evUSzU<;z#B*|>Gr9`DffEN#u1ds8nR=nz=Vy?%Y^!3Tf++iMB!%5yDW z`qjJ_dg^!iL?HuLH)@^>7aic(p44;=^`#8}M=1III=##323oBMjKl@*nBojiE_EM)hdXsu!LO@T<}E46j|DOs5FFsrA8-^zKG59X+RXyHQ-nThB)a` z1Tt4bS0WZcoCEPbE^RGD>aJ5mE2u8k05UVvXLMN1#0e5&8NQ)l^g=6PAd#oiDEjXF zB8$E6*DH5{Y#$B-dvnb}LMDgq4USBV65>ZjiIE^ElMHM1e_;4T{bH&1<&P<`@!zx) zc1Opi9{vKUb(r`r<^A;eRPS9|H!S_(o6o=e>f7(W{aX24{e|kQlV5-P&DURl`SHh} zefGtdUw+MB&Hr{|Vn#S#Ug{Ftu`BoGt4}>WTb-rtzsieZ70i&#YzgnGVI3?Uke+!X zZw0!SEF|%LWQgCgD*E0fQg^GR6=$rDOHT$*9Dejv{+b{1>R|oex_JD6y-6GZSXSNI zQdgLVZ$fH9YznxZl*n1RcppJsK+0LH=pK#|9RM%T9|57!lTU7GUWhnf} z30!i*GHssFJ?IR?N`;U?p5XB-jza+<5MBqVLpU9b^^2RtP@=rtP}aiIL;Unw zTe_Mp;I`r+A!22v>L^DNvNCzSiINWjsX-&uZfXOy&eYO#`s#h$v_~eM+`SHt#~6Kf z?9m8C`*c14t}y0a56VQvtv-`vs! zxcszX*~0I>`sUjO^S}S@>#x52@+;-bqhEdd{deDg`{^g2eaeaW`pYlB`S$De@fo2( z!7;fdEhn!(X3y{Q=QHQ4GYbyiVQj!qhl8M+ERg?))~lI#`ah;Lyp|~~L4H!1sRFzo zLn(Z@ySBBf|4`?(r=w?o8GkWvMm}h^X$Bg>qub|C9PDmxur{}M9T@1dm$4>_C4PxW zrWC+YOlb?TJkVL8N&=#i6vY}qkXymg|B!cDD(9697%N&&9jc|wlgCXo02k?CxZT~_ z(cKAKf<5<*Hg2{Z;JB{3sl7|pXFS;5)gx;Up>fQuom(@<#1kbIFkN%dAxZi5N{v#j z70#%t`ZA+D!h{et1$CAe0-2Y`EX^Z5x-ila^@x~{a9>mj$ng>_8|w3GHu)^d&EesRY(H{J{(U;xHxmoAHab<}0(mzj^V3@cZ}ST5U#h zY@ny}Pg^!_ShsrFk3TM0F#o&nzF)9t@sedr=70aa@{RfHZ@&5NheZpvrsl-Nr{t7d z4_zOcR=qa8ocXPxpyJ&4?<5bI1H5>tyeM{HHiFfV#pgZmkN_@aK7YyAd;RwJ-`~D^ zI{f6($aCmUrvubxcbYHgMHndXhaSlJ<{J?rjtGh zS7ylSnnHwenX(O0mH!(L9w$8BZ`q+JL|L?Vl_YrStb{jTYACavJXrJ3i_U|usl%C?=gMF~)x;onI)_MnQ+rf0Srp_jZ!3tRvjs(me zssXGogeZv+mf{P%Q7MJ71BDDchcd%~I$)`+GXG5D=g2;IWSNYG&6cdXwD8avFxje7 zA@mEw#Khr2b&atrErSq&soa&qdPtudEN`ot4xI(=z(tWziDSYOhOf$a)x_|P3%7{m zLW889CzekVDU}gDbQu1GDjF~c7)6;VaFFW>^gt;yW9O`u{ey(~neplI(w(apeEZes zpMUbn#~<_aqYpp)Nco`P{Xf6|{zu9u%17!Cm5;kV`RudL|L3-Bo1HN^07Z7bv8MOb zJ)+LU4O?n<{e>r^!;he0K6;`IYaWj(!zIGSVIcEIkXMHDnawIA&ryRu8N73{x2>@- zCoL|}FE*9htkl|OExd>q1ay8}fUaCPd+PMXYuB$_`1Pp0fk8T`4IVG7g3ioAm>}AH zRGqwQg+{T-x?06@M24<}Zd3$p)T0I9T?6nj!=%Rvq`1|R)sPh*o6=^&C6Q1%}c&hzPR<-=bwH0 z*{5;>K6?NC4>$lHeDvvOpMCP-haY_O$;W@3fZUe$I{52FdFeTY)qST4_|Ko+ZAr_q zUWW5E&Vhoz1^-K4k20yFYva|ZrcnROWKfwrK_;4+9=mw7udQ6CPKyXiNN18^Y3^z+ zxyHZa--9VWx_{@!tp{*}?_51|puN7*s0FH15-;KmOaVo&&Ro+d6B0T%BB==dVDHKJ zh252u-PB;B^^zec`Izf&scP)$YhcYiL4RIlKfXqdY`B8bDsl!#x`)ACUtM9YX|ThY z?e6VtGUkKwm`UH3xs^ozP62SNExiN)mfy+mY5OU=ndHzPSXt6Yp$NAW&gn&REI<;va1BjkCWDf*~q&y-i?uYZ3%_Hg2NkQ9lmFJCIJwQq^~$}1y4iX+_f zr^+i0d4YT&34#B*{`%$H*(2x4p5dxF9Dv2`9v?TuG5K;$}Xo&V|_oz6ZQR$_0^E zYOF|lH~_r+oCiQ~ZY2FzjoD)D>9-=@%Qu6CnhwBo@6+yaEOn)0n4cC7?y$K zlXMRB+RItK2#LG7v373N+}b(fiOG@T+}K*1fLP_igR#^0G1*bozN1BKL>Nygz)WB0 zG{hkaO(EEpLh4tSmQoaos|G8+42H^sCZi_8ZMR=SowXYMu*C$PNU_)`OLRbpI5*IF zQR{H#jpq8+?n9@q+-R@TZk#K)BQsOgSUJ%hI9#TAK)Y1k z$_Pr?aRc{i5?=+-ePv>nOaQ7fK7OyuY&$hRjX&S5o!@@@B@g|R4?p7fBMsZ{`-Ht#~;4;VI+)45U$Ry?l!B%ppHv2be+CCeE)P&c1iDp zp+P`pvHIalD^NEJc_Dx_!%tKrvjFv@LyzxVInh&HkQf;lxZl&=%L7VyaAZtUPHtLG z2}+HgHX89tgf=i%^^C(ZQsOafk8yb4iX#z{353(4O-lXg20Lq28`!K&S6PXR zQ8t=fjlskLIDGcn{UVAa@1Q6m$KlvI+Q5ptx%kdvZQNye&0XEOj;^ z@e`PVNl5Bc_soN!+yLV+aI3bpb)tv20pT>%o6NR4aWbx`s>n-DMO~X5vLCuCuP-9e z@$}zO?$slD?CEIhYWsuyM`UOM@#onKf2KvGXXwk&5<~l^EaYo|>~UbLtILWs`U(>Y z6tLgQvLb8*%?C{tW?go2PEk(}WBI09=;Ne`dek9c3FHsxKqmHGXXj{ODOl?5t$imi zVy=Tk60$9@9e6lHQ_jHD(-;5v`}E`4zr6vvXX*OuA{o}ccmdj@ywFPBErV6!E`>FP zzQ8`O8Lsxfc=q@}eb1FwfbjpAj@Yqv)ne-XufF{J<4-^Pn9R@9{u4F8KhMCQmG@5n z=}-L8N0C*H&0VBOW@}9tnB^H;&fb6eeC$$XQT^!`zrUjY6ULB0Ofo+g-fTa5;pSb4n2&GY9s`lD8HF1I z50l#a{@LSw?e^YFgO4x$dLQ_{h!`ti3w8ep%aJn*YUzAAP9&x%i_G-hc1CfB*OQKl$|IPn0j-3G_360e$~>-!EKD zH|T?PF$`KAj8k1kR({yVO|B6|O=oYMuFW*ooxFRcs~VkHci(|ShmZYo=G^%Um#$v9 zcI)PyyJ7=<^Y*oiXHRz5=(Cd|gTf;q;?T!4f+w$=>g$+c2mo6y`_0nXQi8idekT|Q z0YGLV;5Ja{Y4!{Pnp1-a@usw}P}tlIV$%FAOj+TxIIRYQ% zv}S5BfYa9IR$gO&Zx@@(=^aG{*rJ(7v zL#JwMetD|Ade^;h`48WI^1(+Ra{EaG@4fdQ|M}tPUwr<>H{bHV@8|!p@W&sQEMB~P z#md!dgRn6*3#+uWvNSKRAY%Q7y^&?@SMHs!&n{~|Km6dZt+S_h;KQo1cpV#C#5B2UxeOtJk=G*vQ)z!!98W`oYwYK%(jdY-Q;80IvmE3T?NQuR2D%52pL`C|0xcjh8 zE-XAOIKa=--8&?eLtvW&&6~^2WI-PEyaru;JySpid%8d7KB@{>B=E5q23S;8N2?(s z1LVLZL#s(#GL~wS!+oQ$aA)>s?{BFsEl3FPiEn8Io)DiyTuMu!7EAxf1dt?WuI)H{ z?)Iah(Rm|XqeC~(Uwbq)NvS`4{iAzI&pUZpGU6>%}wIr3U~mmn@(e9aD{K zo<7By6M78z5ZwWZVN@A^C%j6aB$|bxM`NRx+sm47j^B#-c*PDESD3IqenBBD9|??z zi1OREV$GUWYc~Gm>=_smosbSc0s9#5?2O2uZOaxdU9oo4%7qISuU@xq_0kpVojjvM z_ikU~oK@t$`KMj(5dpk+5#h1$ub9Q~0P31rsg!7QD4o$f$uu0N%M7r4d*~onLrJa6o8yOiZM|*Z#0DU)P=6 zo!kOLS)zwsK{%^}atlfdSks}M!!kT(wMtnVBOd$&1paHZcOv8xXo#6CGet=`?A8V? z03s%u)8fG$&dY&6DjTgT%!Z68|Cp?Ny;9z9@3%Kp7H32RrL=blX2qrrdRnPz;2+D_ z3eSPcn)XBI?hK7eP2Uae<-yh4BhQ~x)X!YI{@~Hz!|7LpztmNjsvFu)UpjvWlts#K zNd;2;3pB*qZ8fDo&;gm@-=?Y3Ra1YXlR!Q5Z01I1h5g>_~v<4vbT?aa99W% z^Wx(ZL!H-e-n?o)WFm#kjDe)ZDTn_d0l zBlhjsxKCZ;xpC7Lr(hq?fPkRDFzCxU`2{TXqRFuzBUbW;5xr}um|-(&&?&K%Qm&IA z9-uT_P^M{6>snjtr5!}Ks#OZ#VY^Nh<-pY(NtW)LDoY$Zz*V`*jc{^s@PoEh!F-p2 zS}9IR@Bl^J&2U$w^rERT30P3qAZ1EVPhYparN*RBi;Ic~4Gs#Aq~Q$p^N))3+qZL@ zlRHI9Xb>C3;}eoIv<1axBV|D0oC0~N(0CZ}2o}^NX}hzxm9I|?K$k9neZ%&B zL9tnh{@#1TYub|h0z(sY*%|Pxn8fJ{0YbTL9sNg+9X&j7ptp~~I((^G^C{dfvUKoW z3kn#G0Of)1n2^j8BB;_@+FPp2i*Uh{wWS4GJ^VN@Q4W9vR%mrrO$}XkuLN~aHfV3w zdVvBAqUi@ag%%^?cG4|#Ao7GLyA;bIS$)ghx7j-&QVsNXb~IKO=cLHa1u6DZsZ4`o zV}m?)?{M1Z7aS7EB4H&=7a5nCUm|*4ggYfduV(}aCE@VpSD8yTC`;8l0^#_ zEm^T@8NXY+a{1EDyEd-buxiDMl`CB%_q(%<8=pI8isD0mGdB}{*ZgY%% z83J;JDG-HO2yK)!g>7zSX;B`$L!z9@ID@;7p_tc({MceK)-YsjZ*FTc#uxN7)4s4R zj5|iW6j?H@E&QGX$1mJM)Ha5)c7l?h(EfDv_T3>0z?b)rUzwh{d1d_N)X;AO_C{+< zOZSn7!q!)&-_6cW!I`Aohb1ZbKAcHnk1OntJ$v0e{DSFWGjnqiI9w}(6Oy4B$OXX~*b*il> z*5_o%zMiBsR;@s%VOKPzn27^ZRV_SUBWL1QvP})MejZ-~-Z|o=%DG8rHMXtZk^|%z zg;#QDNbr7^$c9J6#6?GhM?_?qvBIbXGe5laKj0t})b5&`*^rH6eHdA`>L23Z1KIk3U zxd^wUeZ`2Trm5%1nX7kh-y7y>%?hjnp7MNnoGkFyzmHvh^tZnc-GBLym*ZCtLlo;d zaN^?Z-++BU<6pl9EPBiQ*Fmyz%w%Cp_A~zntl>ZT2)YOdQuF%7EN5f*=Y|7MPkO@K z+vCLwvfy~6FG_9^0!}X%;sBKXcTO&Dp8g>*NigSglj75&-S#+dUbqN?8$q2>~?}elcf;5J8q{Q z5~TQi7Uxv5Ni1&p+Khit%)?>j0M9TrIha*AM zK)jL0+7$NjV8bj5JxQ$9b>J(c5v915{gJipoz1OC1+neMJ|#aVJtM2I4C^aPHUi7Y zFk*jo$bJ%{UqC49WURn?)H3VFmwPGW;26Z4<5vz0V zNN>VGA8#3klg0uz&g0h&1rhxg4$G(!@tT*#q7YdM02kXimEs4{TFHn5|8K{NCran4 z#(F0h888YK3(o)@B zomMQKKYt1Goi!UaZ2oD-&RyHLZQVp!zt3l{vy02F9Xo&8w0Y|`Dn@TN_dTBOPS8MC zE>wOnF8E>og2l@g@_+o-yH#6%TEAh__I>U?(JAS9MdeK$-QcM97I3$=RtKSmW=t+e zNlsd1U}#(_i&td#8t4!30`^@}I#UHW)EF2bPz`fC3h_fI;0?t7O5Sn|%V2zf1#=jx z$^N`~naK(9>1r#EH@UgA0-*bZUvh^m=3#)W)3WP8F|<*0FtW#xi#5J_o}^-BY<7wEUr zBnGbdOT+x7OmuVw%z3bus;M`X>0~hx3q0Vg$ofjoG2aYTHA}-X3%mNed%Anu+d-%M z>e+FD76AU@1x_#^dEfDicOId;o}PSg>-spA{Iln`EICamE^i z3=PL~9Fq6w&y}Uvn=lsA@$emq)r7j4lb{rx0s^LP zAW;ftASHPNq&}K>9a|9Cwt-DjZf=2&P|E`oOG|03m=&@6#8j-5YLR7I6e~6!=2Adc zLX522$!-4mU|VAeT6^}!MudecAytXlP|I>NQj%FE8xx<%S~gNpDN!JQ&N1psJS1)K z$66ZEQBxDv*2oagRBot5EHd(3xx{7PSI7N@ZkA#5_HhhbFlKt%PH+U zr1X~!ba(f4bsn&ioN#K=sR{?F9V+XZyAGeedhf}|#PrP2!@+5gowt8|@&GV!{Kd1$ z(b+eDc|9~VGd?u&8c{Au{Q*p&t0zv*vSSfY9y|zGewM012>x(FU%e0psQ4?)-sC^A zV0bz)!qUDQmrovOC_8iOY-Msn$d0uOe_XnJ)v9${w(Z{M?c?X+DrvykBf!hu!(-2G zr(HXD?{W3;4GQ)1aog|ZvTehLdeB;K|Yu0XYc5w}i zOUo@PZ)_(BbamLf(Jpp&$Ps8`5`$#7#;8k=Mc0l`85BP1H8v>FD&X!~V=iOSE5?U( z-y8wyz`46dA}zBZP!gr=C>6Nm0sz-~E7BaGWKC9DY7W<)%LQcwRSg_UsP$w5SXQvG zXc3gs(^6?ur7U4oBFMoMC0a!%9H105|D>eK{wONIs!2S|mi5hVu1N7F2XvvV#B zhu`A41-Lif!Z(B_K>Rc~X9mt3TUgTA(}VahJvSfvkBoykqOv?un-doi>>m^dMHQ(J z&_^!MxF9!Mos*Ij$5%78D;@8+6s=6Jtn?Z{!Eo8AcIs_?r!L)Q5c~(m16kts)qA58 zGlM6Ok5Aq`cWZLu;jLQ_NDae7!2W>$Fop2N!Gfvd%8by1i2K8jk$#X1hVMUsBP3`L zzRTB+A36T>&*)`4ZB_cbl!*O(d$z2D-Mwm;`;HagAm>}ZX4#Kle>EQ%c&T82D_5^v zwshHwb*on`UpW7V#me&G6|2^)-?(YxmK}RS6T&yH+_Gi!k_F$-U+}{Zi&rjNH2<6V zOIEJj7ZRUtC=&-hU=hG^N^k(w*7g?4CPB(%|CG4g3%)-9byZ0amU z)2u6|6r$O$lmK3JQr(#qigIRnGHQd(jnR*qMHj4b5^Fr{)QBeFDh_*1 zIWn@bOQ&I>2D)B$#c-q~g_M^885I@hXQrkkCC0~TtqsLF@xh^?zCIY3uthBLXZQ0igmn>h) z3HaulAAb08@v7x3R;^vXasApgtJiGenXg*1Y~|W@>&Wt3e)?(a)}4F8lVi58-@aq} zN|M0hCH&KsOBQef)~xY}Ov*x)P>-*e)k+&+=i0-iKi}QPd{!tG)L~+ch2%OWz|%cE zj%X`edo^mx0hs_Icmq{wtYt_>(gi34U#+Ua-&L__VRbdAYV`C3;_R!`*(lX@IkN65 zN2r&@73D%0BKi|GU|SlHTv-YDd0E*S5mW;zIMBPQN<%S6MW2_YWb~w^7+P%=Y9#oP z!EBcf3khTQUJ-=*V#a#7NnpVb2Q|lJvs>p@%q^dDoTCSX!=^k;qTp-`-?WHNuggo% z%q=M5@e!9S2=*}~f$Piuy4aW$i0TE3u984c#v^p-3vx1(l44_O4=4lo4)pb5@X*@_ z|Nd;2_|Fgkl<4=#JJ)VryLR(BW~%+Rx{9Ja z@b3`fy?>bB?zNjXtXscw(fqHzUhu=hrO5i$D_j2GrZuZpt=_m{iOey8wRpSBwyUod5MFfBx``Z@=B@84?4IiWicM;+fAw!D)ts+YS>) zW@!x!ab&Jz&?biY(J~|@#>&Ea4D7VTefAEB(glHDg;J&$ek~B0V$QE?K=?Z1a8D7` ztjJL;>NN)P1gHTZ19DJoe2SPO5UPbrA}$*g77+5u;({ia&B!cSfx=c9 zw(BGowzO0hWGHdA&_KY>l?}!cfZ4?!0 z;h|?|C9}qnEpqI3NJ);1Ndi01Wt`1!Sj_v_>604dv(GakF$dl{5V!{W^+G+Su23H1 z;t~=wY~3)?#d#B|S{*|J;$3yU{ou*VcODVJ#T8+6V(2QXw#FtWpWMB6fB2E<*0q~A zZr&NfBzpu0wvn+(w7U~z3xYg^pvNOm8hR(j!I8)Vh^H{ny>{kMedECSt86koda%7# zld3VBHHis{3DN$0_wC)feBq+Six#hNjSF#e+O~De4mU3!&t02VEnT#H<%+fIH?Lp4 zeDT5`l^-6IlHymAbLKS7Qa6Fau?a z>e=MoTepX1Uc8uibmz&_u?KhW-@E@{>={8G=x1i;xgbHR>C!1iLY$RhYv07vv2oSp zJLbMKvlDkN9&2nmbouTraZ>NM6=oLJ))XWr#6^dCY}>JI{n8&7@o<+oM+Lg>{%OlL zCvX2iAMoB)%h#-3vvKp*EgRRaTqX_wqJ;~8T=*lIfpGxX&&n-(eS?D{-2B{Jc5L5{ z1a!B50IJLAWGx1$aEE$(v^^l+3a%J<%Tfb0*5Q)|N{7uA5FM>so;oewf1giSd_qc! zDzz^y9q5O*4a4$0Jr|V^#KFZZEC8nQ42*ErW7dmbF(gid(>d?e!2OyfOMfvRxk&+yE=LHS$JcMcSK~6f1r4lC#JKP zjm}L2G26tHsm#*QbNE102@9R9y#qb}(l*CLrwTwtiwgN$SC9*FMJZrE6$ao*38{G+ zrJzBhW{o?$sj@OMGa^EKe4>(>3Q&rm>(?uWO0aMM_GGqMr8W=X;(XvhzjC0fzl*)g z9BH`geb7N368J^HG5hB2#KhBQfBEb0FQ-PIy?rq@q>Po1&qDtP^M`c#meL-fRTairK^|Y=8DazQayIuznG~*H9nGRHjN;9@O(Z zf6QDU<5H6`Cobjyh(?6!fvZ%?Y)B+H-~lYTpqwy64FeUf!@9Y=P@M@3fk}2+LSj;6 zXi!M7pMPKk(HvwkD;M3c)Re}usy1U^PlGYPsG`27zjJO2nqNC*XJ)=WpU0S%f?rp1 z77M+|0F3@I5GD^`+{pg5U$YRhSZWPX;EQR6`G9Qc03VrfK2uY&Xj^2bS57(6A8`s9 zpb4HDM9@P&pZyJIHVl}@9}~PEBC4Iae)J;iwg(?PxHowB_I(2T&?pffWS_MwBNF<# z{FtO-moIqF!{)l16GMyv9t}RYfA{j4-m;RKftH3kh7!1hC&UCtBOcLPh(?K zGZKQ_owluCw8SYkH9XMMGd4XvGau%UIy2UN=dOLbw{70Aam9k~zyJD+Pd|b!{*C1N z1xr@`_}zCawzztFdboJ+0XSlk;uDgl)0t|UJNpiFirNO?UTQRTqtem?a}vdUX$kNf zlisAHluUZ82%>FBR9q6vjgolFlF3MkDd|d@iNKfym&UP}IS1Ab7WP>^yuaDGY4JQ9 zS(yxK&CHjGj-Q|SA|vpn1D9vc6i8(rMm7(~4C(_XMD7(01UMs`3`M#@Q%UVkTdvn> z=^JI+0`!Z7_=I@&Cnl%yDD%{^;+ws)i1P|->_kOu{_4<8f#5^?0nGpUb2oMPX z*M@0kz^chZ4tKUu{L7SWCwQIMV%kJ$zZCkhG#|7Vf`T3`xef7-` z^S}G%`-LmkE?%%;^)?S5U+;YZo=z?#sJvX2s{ z*=9wTDSM^W|scQ=7{G^e0I$*&_<6UMRI*OyqkIsl(5P!t08HP%BHM=@Y5Q0LLg zK`dj%AKyV<9&$a3KAy0AJHn!%>DO{5+RQb^=*XzJ%o1dMxh(d8_vSbOya}3oj!PLL zsD5RDP~AI#-*|m<|B+MYuH1S^4KOqH>NWHHXRrS;eiLr^BzF8rIwqzl=%2rO^Wr6( zpEs|?u>tId8lRLgh0TX?cB;2^@D-C_JauN@O+OhtXSa3!+S}R1M7< zB5b&7NvUyx?z^{bSS&#x)OVk21m>f;`S26dV+aCn&O1ScSN`z*cVB$=$wv@{z9$NN z{r%#VKYsh|^38jp`R?&@-nenoRwqxl{n2S@x{})Fu3pg1X7=FJ)pZJg!e(#p=zx&l zKnGq78!wONnuAeaR&sP?czAStGJ#V`*0b+6g$1ZemR`K9dGLz*sNlrFz*JNiU(dUER?6 zudM?uO>n}xZ#ankL!(b_-@JeM0({Cdx9;A0JagylFXwMQd4%yL$PeRPA^8mJ#32C> zw~==MM$8LFN1r@-c<(d0*Sf+~;ZWaj^hs0hIG#NG{ zlZ1dxUOqRNl9GO(;(|}^0C|M7LpcR8r%H?CIm}(8-b5oI?$qG>gAX_k&_!(cQdJ%W zjo<`O%pubwn3G?`h>52t@I4r^-cha-6g9*h=F@Obi;EErS?1gXlQf{>-~r2r|FO$e zo3G}m$}b#Qc#QxNtQ9FX6l7&+C^_PzV-wP{Xc+mK3#wU;2nhC{)^^yu4 zBx~`X%H#;7#-~xHK+54#Bt^lEboU4h5A}BU-nZ8$C^9a}-+AM@<;xiW&0nxYS!>?q z;q4m|m8OO0rRSLS9D{PHbmq6&+mzNx$dh(^M^{^GD-K)W=G;Q|x8x!DVl!DrDtu{K zTbw8>Pq}g=)6|s21pYf!3d7hq+HYbhMF0`+yX)iatxAUj75!AdL!j(>pkJbxY_ z@f^K~qy3OtfwI2{dp0@>ZcBODzgXrd#&Rr!VRZ2Pa|rBofl5Aaf?AV1FQ-Nn>Oy?d zep2F(N5QK5!s5B$?NaFBH_eZ6fxHy%C2u5DuK>G0UI z(c!7FM-Nc=Kl|U)gLfZIyppv$&%pRW|AqTg&Hz|~Jab+;0%)Kr;rmREPyKrS!JXST zFP%JkaA2Ujp^Di_efvOjR%%i#$Trt1z%MvBB*@PTOFDGX&f8V{k_Uzi_85j{2;<0tvVo+b?L|=ZtY}xXyF0St0{*kHV?wtJc zx_hkyupL@kTd<}=ePx3`!62ZMV~yjqO%P7j>`1Ll&kxBkCXy>28%GPx9Y+S5oFah% zCp=PdVvMY_&d5}<4J1rS8B!Evl`>$J39C4E<`_CQ zj#JVuQ|9tLpzO~|OM!VBP!9aV8-!F5by4llBk+Df?1vxS`0do8OOI}zxjZuQU=U8v zgU904DY`I0bX1bGJYQc0BSIz{)6i{qzsT`%kwd}j}8lo%1q5uCq;#YM+XMS z`1=R>y6oERyvyCk&BvdzoAH*KZ=B^4d?$=dO9W7%K4UMLQg{lNc@5QeL||}uY;iORMTvph9+G<%()@a+ zazH-}-f&lriHYKgCsN{*>r>NJ8GTYP;d+@69~&E&7|-513^7@N%^4vfaj*D$DWfGS zYc#678ciXg-UtY%muU+qYz z3#Sp!_<0PT#m~tAYjuzqLbJ&`%KVxap-m0*@sCK#$R<(|ukv}PfwFQ?;+OaKQuO!s z_4IUfwKtxam=aS?`nkJT&!4z8cK_n_iK!GCwDJiIQPq;Lw(hSh1tom5n-`8nOa?XJl@fv5y@epVg8J8_c*!lck>O3U?D8_ z$h<~G>q-eFlgUJB%%}~yhn5!&NPPwC`M8Lvg!J_Ixctn}q_`xlQ60U{+11s7#GtnmUPC*odwXkJ2N%Dw-l8&JMxSW%6TllmWPbh&5 zWNieW4J!yfyEZMt*Eci{2THzgszODF+DV(29v^J#Q+kf}4ICO6=JiritDu})qti5>tl^fBp4% ziy3|#pk!fXX?2Y;51b(?COs-LI@Ht6&DGh<&)o-#OLSav2ChkFzGDt`A=a13Zj45` z2ym~}d_O0!cMT5+3Xh8o56?*rNs3R^8S^4`|Fpx|d55>Jvv+ECQnrR2rm{MVF>I5) z_rRfnzOJs0c2%2* z)u6(o+k#nE$q5(c1~yRDs0~C*D1OW`C8U|qY7})D!y7$VPa%R2hR$HcCe-2O)c^!w z3}U$q!RudY=IDTSdDQb@m$7)Y@SI*|*IaZykDgP*hq8YR3<^G@PM4dS5EV;|rNPQ# z_ns~%IozLh1A&1)v3AxPb6e~!s%CBd)yTl z_J;?!`Nn0kA;wTfvg33r`Vl}eA>#4|VvHa-8&(D^5RmC?eojgp!*5?Nx4paf`bA~H z+An1;TUFEAXklF-OZCJ=wOO7q+WHoRcQ{cp*cYNSLOwET8_d=$i0~gnBd#=IU3RHp)(R~REl9;22+quvjPpw2CP|LU z@+x!;4t@Z#|{}^t<;vo(&(j0U70c=NSWjF-HNO|1ak9yp6nx zj-*NcFOt_D9TDj5wr8h{Ut~tE)=*K#*wxzAir5~3ETcRL@g3~~TY_35ux-In1AzpL z{(9~fF_v`;QueZI8f`aSTyzZSo<-z>cTy%RIR-8gdJxvxXQ|Q)q#I6CG9EKEBkFU} zxn=pdqvL8Xt(^!Z7y=^glgSNy`v%H;&YO@q)OR?mUc&nq#^ zE8*fehKmmHkx3KN1$c+z_G)fykV-`CDhlYI1k$FY&0=o^3cm!Du+gzm0dbhn^z`)$ z=-=1d)pBhTy|+lcr(Zn7wB=u)o0#On{Cp+o&T|ob&$1fv9U?EW)_gJZbQI(BJ2&rK zJkSoGv8J(!8C#7yDFzO6WL$Jyt~%P+ZLh1l>)!oAVf&nSJG%w>y7|YX0%z)r;USUC zIJz82SnGnKvrn664=hR?ARuB~kc)gZBFx{*)p@(KZzKdLeTku(x8H25uVfc?E#?c9 zb#@+qTbt5uwzqY%iVtR&1Oy14XxVtTamSYl1E@$Ukv%FrDjI;Dp<1>o3y)SuH4pHE z)p2IJD!G6rFD^+*nN3f3_#blu5FpS?gPl0~FX&uCvl8N8xg++Vw2^5dU9BJr{J8*r zT<{v6rOB}*7z!dvK`6BZIHo?;#3Vt3iE_Av)$rm->f$g!EGQ|jvo*J%My`M@Y9cra znjpq~#1X*X{JEv{`NetMe=6F%g0LeSQ3c0{44l9>j~nuC!4mH&gT99(w{_PifC~TzETRw~`28^+W2xvwL_P z2OydAK7RDz?v=C0dYbDoNlJ-LNr_KPjf{v42?`7iibTQ{!ra<-zsEkOU9R5lE;}jq z+zA2Bt{&cj(P%2z1i;Cxpp20-&!yniLRFOf%{i!6EY*0DN)MEi#d>%Y`GSEW$b)cI z=dj)ojU7=}v`>g1>7|%fH2<5egvP$5&E6>!Y#YXihw3cFdUaL?#D*B|eOP#SSV(AC zOg1#l*tPix0s=$#3ZF8CZ|V5MTZ9mhX>1(5918@ z?T5&_%XPoU-rX)9KJG*S=e_P;fzd@kQ7K{pYEiJlPZqNAe-{A$*bXQ*NIQN+ughZw=Nrq+a*CHdbm4WiA{&%Rt{hYc(W zuBuX5es(;1!HCIpJR)itv9~dnh^9oMA2v>=8YWXh>E<0Hq_6|FfUM7?&y+7EAe9on z6OQ!BZodi*zsML9;yxcwSJ|MH9+K%Mvub1pa=L+jXr=QKW5Ou-8wiJ=GAemi}*ySWCAIUqM3 zE0aX#hlz2}%fdqfd_AG(yZd?X-Rb1&>AG+4-d)aQ{$LG#Y6YQ=#W2)Ngiy*@F|UC< zEAA!2qNH9Wgwf-w*?*IqWJ?5jfd$PLZ0K`3Hb-)#G0Vhh2PR*0LzB{~*p>FnV0kT$ zBA~N_k-{I)PE%upr5qRTtQ6X~l*H&rWHu3zak&6J2s?T3py{w*W%j`V$k%9dfWgXO z-^9mZ`33<@M?tO_>VRUooIEBTkz^hKJ@{Z4GBVWpxw_icUWqk4J}Uy?Fw(_|mGw@Q zwS)sgufzD2lOpb5Rl-5!;ZqtYHAjT?BSz{~LMk+N?-zIuBXG~i32NeqAIZb0L-OZWFSxoa-b?~fm#B2 zFgqh5HX=MM%x9mwy0i)kY-c;#ND8Koha<|PcaI-of`yS9p%F^BIU+J9 zHlF2Wq2ZBo5@I-4=}KlltRHH8!AX&BF+T%dmH|9tKN*_P@H=X6ZkD{z5~VDk_&(1( zE<|1zOh}5E`z(msIh-ojn zJr)KS80;e$p?m}SFz6Jm#ZXq9o1#WZWTC2(Z_ZkW?{5sGB^xnkwUpH{Bg z6~EYF|TRBCAIBnv0M zxJ0Nl&COJCO;r^Jt~rNZT+Etn&0Sqeo4$o|4>qz=bCeq+^>u?4+C)8mD~&|6U(X@~&yWUd0b3Ra5l zH#bw2-jWHtOU9HMTh>@{|HxL{ZmHfF*s#b+DJ!Fkr%r)r<9PlQ>rbG0(GnVrdclSG za0p>!4tRTo*g?qlB@99ETa!;nq^o6RF#i6I=T8+(VFMRfHi}DWS7t8H&k)jLZ3EX+ z>C`qiwu!}m!yH;-BuIa}MHoJr!L{V^V>HSa#WhkSyye9-*W55<-K7QvEk`{gPCD5< z)CBgs?S**Zwm%?*r}w3b6J_o&lg1e}!33yVQNm1fdM$>W4xPnMtu@5qb$!bb%FM zjj&8l*hQ7PEIoZ;O+Az)rGv%jb~ypoIi*F+fQcx)Az~|}R6Z8vzg&E21JM7WHz1oJ zJK**cs0^}%2uqKgtjv`7_%s%I<;Mj1d2wU+Mdvr8;q4#?w6?VjJVfB};KAUdM-Lyd z@Eov5d2;139l_w88|M!9cQl)g88Kl#^kN$~Y}vZmLA~C#!`W%C`yO{6f44omcOcdX z@bmEW^$dum%t}JvC>vve&hmjv8;S}uqoWfU_h9Clt0#krTct1~5h^If2Ov<@bJ20E z;+?7znJp?d@DR~o(ZUGy#ht_Y2vHO1LTUH~N`Hgw&H!uU&M-_U>9^u4AP3QP$k@gO4zY~E1d#EpnSN{!n1%SNnMbgqvRUs{Ui_oNh*dz z@csqW5!sJpuL)F6V<~sqg8h3&My})91M^G7pmRkn0NGnFRsg8ORi#%s58_-RxKEwz z@PGio5SVc7po*25&;sHgP()c*tVc|SO1+SkvH74+I>;~5F|h0%b%teb4TdnVmBl~KTC>+j|5?;V^Vs0#Cibht&Bn)03yvMRJGDH(}TQ3BT zSaPSNkEUQMEhVEjAj)jSWWKhlhk8)nATCoS!wn^3eXr2e_w9-68rz=91mIQ1zcpKDu)L z#Nqa)3Y{h`p49H;vCn14j&0kvZrkqU|x%)+9KbC}~4Umo7 zPjv;nU!>1qBSvg^SWIG84yCnRex>qf$~kNYONuQG)~1g3W-Br_E_Wj(9RNA;9*_`8 zwwn16myIjHUvNaY{P(f0!bwG9mf<)BW0Pf1fy0F57|Byop3#V4Ovc?6GN_G*eD zEn~U*YI6x$HJ5gbAB=EHWkVa!lR1{$P7)OvfzJGjv@Aplnw>gX*aUYAVvsOE9Eb<> zCtzX_J0zh-*~&qnqhl~K>P3|dJ%~+jtyDM7(7bs5rK0F2RuGRQ+bHH5xkrQoE;$sTQ||)-qw8lKKk9k z`^v)?gKRavef83>#}4&0R1{>zM}`K6hDAn(?00kX@Njo?cHZk98Xcd)WE$**F=rnpX^~{-)>r3wvwh50_4A{e-^6;Qa(SB?a3xY zi3wy`8av)o!aoH+i3wm*b~;$fNijs`GR06Ub9e%{pmoGGk`Qt(fnZsxas$|h#}4Ml zbO0QOXH3ycM@agYPDP4ez;9#$Fj13WoTw=%Arz|fGSlElfk|`pt6D%g+PeGao}7DT zPMKEPBceO4h*-WTmUDb^>OLwB(IF5{D)FqW1Yn_R^|2$`$Ezxx(@bsD;*up#|Q5{6e91aV|ve8bCzBlX{#$yCq@MN`UixB2Dmvn?N#=@ zbK33X7R)kZ-f$u0WTYj^VYARNY`} zkWE%lr?KRrce5eIXKKUf#>Oc}c-GP00iF*(*HVy~lo%cA8x#)rt)P`b+JwZ!WOP)JtzjHs zcmVUy!L0$`2Xmngl=DOlPUJ7YKcIsdu!vdo&P)wbamBfBcF5nepLbc_W``M!qtTGQieoBK zhci&lzGg^+q6$b$iiu84PfrXFiBF7=bai$2ic!}eI(`rdcu(6e5AHp|#FgO=0MOI~ z&gs{#T{_X;xI>UVs-yB4++cvxs`r2!$N($8Ji70WlZP>VN*Itj{xU{rnSp=V=$;iudK%_{Bp%>zl zkPC}L=&<<}+$Ml^(!NFr2@nqy(@;6mL$E1gQZ4ANY_t2L9j@m3*CWp2!b8jw*@4_| z3i_s6qC5${5#tPKkQyDE?s*Rd@L*0v79|UFd}^yzJP6)u`R)MefZOHasi`+A4iUDMztmM$3sXlf}(uap*Ro0B{^7 z0qv@QJ;Za4$0fC+Vr&!Q78q1oN{$>}nMsifG^iiI5v6A)(pA^91js(eBI7fLS^$|4 z4(TxE>(i4ee;feuCwGJ{#{rPJ6OoMZA;2oox*E_c1tKjWz}?klm(#w0BvVrlFmUI^ z$B!Nk!rVG@;$TayrL-V7IVN;JE*l%RZg<-2ADx_%90LP3tpMs!W~N*=J@2_*fJ)e@ z0$3L)+Ge~(GP80CU!WT0<sZiYCR~ z<;ELuFLD}S0tSmdf;G9T@_cN)rGe15x(Q zJP21NdUKU&xG)v~?Lrs5-MyXlwU4WsTW@}H{iEx5pAGK6_}%Y*=gpfx(y93`{`J55 z@BiT+|Lg}}eEaqJ@%GBhP;=EigL&>hsA=l#R62fI*SO!?+Sy4Sr-GI`isp&FAzG<0 zDWk>k|Eg(HRZUUd?7@Y_grFTC&%HOoG|RE|(xod7#6NuR0tEi|CU7Ymef8$e>(`V! z*t}PlXM0=3U{iCGQ<^1xG#M}8a0CDp$5^0JZ51LhpPS2#FqR4M;6^)6aE*jjKQ);{ zNJZ{)O`RPWfmTB{8e3fZcFAFTD^TqIM+ZXIr&^*%Q-)d=WWn)@_t~8D54(o5U3Pc* zBLBNBuo4&OlH>1-)&I@4#$Tdx`3xxodvQuW`0}bzJ^rw4eRD(XPw@u@#Lb;A{fucHI-RCH^d_+62u4;7PY|h z0Yz)k@)<6$US3d7h1S+(Vq|FeTzofJb9Qq0l&X`|L)OLS=gv)9E!H3fTzJS!ikBK5 za%E;t(w08Il!4R%X@J*2KySyN_mi0JcCl}1lcFcACXqeR#08Ssw2dZ{{$H8#}MdbQf2ZVPG^433?mOC^+xlLDf-#h;*!iK`hyiVFf;Kl zjTjDB9oNMFtgh{?t7`8YouY22BaERDAc9Ow3pj!Rp1*wc+Fvg(&ra#l_W7)E9Hy}# zgQBic_f7dU&$!qq1YvN%ZLsz9JY%aY(P~y5~F} zJ|Y^Fki@7#Y(-+h^t1}s=-A9qOU(>J@Cgc@s~ zJTQW%wyB}9o?!dKPj5cZuZh<~jE+t@>wwKbT~id{Fc)KjD*~S6!H643c51#@+FxA} zFAYdu-&>zqSl>R7N;j4Bv)bbXyy+pqRs;Us3~t}VP1BOwo6C$dj*$88jc z6s}bgydUs|1K~O4spY~~34&QPIYjdg+n+wTbL)fO`O0zh4KXc!!~8D|Bn(q+-|Vy+8Rov zR4y)$C#nulZEo%#WFT%Ps^fS~cX{@Ylo_Z9!M-&}N?Uf@gjtM#pT0A;9dH(8L z-}YHKcG1;|gI!yzSVfZh)BcKzrr7=rUmVlO8Tk(GCN?OLdk(N@=36`dr1#+jDhBBR zi|zyc9gsk4IHGL(u?Ga_V^m{U!~%Dw&X-H$2RcPTdb47C95q$Uev$MFpTPkLP#MK! z3`059#m5ugfi5xL;c^}8Yk7z3#NR7uG$k;FJGwqM(q41#_WkP0C)H0L+$GId zmYNyt>D7vxv5^Pd)r>>}g$`zuL$&IJnOSn8Yg0p42d)lX+3>XhR&66ZhP7V8&04WN z32b_s<+!(Uz~y@jC|0IYlg{>ncZn$Z$pIPtEmaTifBL}(-}>5@{@Y*u>%acZx+;3x zum9%X_zlhViu||kwvDYFmnKG>sRI=7gqGGrLoi+$Zop>k+(`Dct2HZ;!Bhennu z$na0A$qy*{w|7s>$bo!zHV+txokJ{O)3mJ^FSo@@^hH$33sTYSeIcWr`8nA)vE1N>sdXneJA%}Xq+IvSQ7uSmYab;NNKX~N@ zd0^?;OdwP^pr%Xh92?{V7)>oCc*LSlvH2GXQe_4Mo4UZc4xAyg88dw8(On+kD_=2m z=Qn=iD|e{HYpd>F|M-)SzxDNRVD|5~49q(6)#?7OW;3YFNtBC{+zXDoM{a;a(bTpL z*(rKPn)W|ga=W=n*=DJg2R|A4;rQ&9o#oOv%}`Jf%o{>DY^9sLb77X@(Z%b}KmU0l z_x1BvZ$EpkJb8NY>N6WNpI@F5h+>K@(&Yu*^D_9COq$)-yT#s|nO=A`Ct*nQNiu@u z9I-_XNuNXjVkEb&iouH&qYGiMd%8QceYUn1ZHJ}U41`smonJ&zhFogR;0-$1V2k5s%?P(^en15HyNj(_-OQ*yp?>|734{ zW^sXPagn7JgkqI(k0=DL>bZ&%0F9Nj0c`+9NVqDN-led}hWrd%BNO0!qE!S;RojqI zE`CA~2Hq&Z9p#M;+7rE&3ybSq!``8q++caATdtuD{Zl-J)qPHQk~!^o!aIC}kb{!C9@oxVDGTayLd;J?<`SMqPm>d;eFgdar!kLZSa6 zr1Mg&!bS33fylAG6?HB{4-TZx7_#h)C;pBb-{s)X_k_e$$Z@3hH7J#yYUBI-3j*~Q zR-s1QeazdEkIFj40_^buwGRsj4-ip+W$46&loeqK4iwtmK<=g@lq)JK4qX^$o8G;5 z6g*Thf0XDNyv-BB-S4Y@3GY4u;8v`?<2apQsXXw}%8>639A^0n{bF7vx}c=0Y8|=( zRJfLAd^SnqzDkG6l>uz>#ni66Lj|k@u8L3xVxuteKE?++T5Toiq*5W7Ye3TDqRgad z>6=|Y+u5Qg-ys#@%9TsqS)hikhPDm~U?k+CX@KWsVJn-v#}P)Lis13oFiPmk2ADk? zDAtoWkEuv(%($K&g=BjDAL=?^4D~Gm9c5jLTm2p{^K+eQ`?sQ#v`o-=Sj4)SE7QDtIp~FQ3 z7n|}abR7+ZOF^~a1Ryd2uGq4PbY~}Pv%mwQC%qk(oTB?R@x9WW@Et8(uiw79eDRWE z@eE-?CrJdDGO+_@0r|+zoACF7=WyC?Te|)8n~vVsGZdFqhA3EDsW6})L-HsTAD{z+iLZH2OBNm)k1Gc@O zNyAX%AF4okC|fflO_c>F;fs5;@q@($qb% zL^NjVGe|Fz7&Sb%mH-hSkeVfHrKivqG<)TX>L)eTRW%JA*=jSjxT)qks^P;FGFj;R zk0BZ6`+2|E4vK8d)RM)~m150Qp3H?s+D}c6_I1=(K6q4p@1w7N>2Lh{SHAq)AK$C3 zX>5G@talVDv4x^IoOcV$o~OjQ&GD`EjcZ$3L@ez|G&Sy;?1uh{?g zA%|RI?wC=+0(vclnjF7d>B5?@!8ba}{9Ua7sn`?7Rf;6x&fQ10%;Q=^7-ze)E6Kw_ z_}m%Xl*}69Rk)4O(OA3aDO#65d!HYgpy%4D*#3zpz-6GcQkL+uM5c82yAa5d1YPi2)AZif4+DW`ikx4|~6wQ}N z)-)sY16L;u1>UczHYr-Wd^yX1b$63=VnWc9a^Wuiw9Eos#xnS53zRVp5j+O|(_xL|C+p*ie8^+K!*j*_$Ca7;;`~9N03*%fQ}ePyySJqq&VR=~0_YwL`Jw5- z3hS8q^h}dcy4Y@#;06=oSWtIQeh-61_gV_;;!7p^DYkm=bTg-Da&lFq(zLP zc260=@o`xW695EckVw!}(0>UL8xTWnk%EaI33neHoj%VvqBpvDrG)!u3Il4tfSm=@Of^Vf!9`t8SK|@X#dPFOkY{l;@Dhe#pRtFpWd%4+7`P8O?1(B2L7*d(9}r& z%Rl;P7=t46#6%Tlnd)FM{o|5w!kADX&JTClnEAB6xtmyXE^9F+=hYF{G8|i2wSLMD zLQnR>G78zZ=9S=XKRn@57P_~qsEf4O+ppMNMB4eXBgq3Q_9qW*9zCi!**rSaOk*e3 z#i?;z!ct)9bg{4-V3LFaWXh%DJgmSx5my1ZL(2*x4&-wLjhEEEP+^YV>o|S35L%mKj`nu z(ePk@C--0b@b2wL&j>RHi~t9?VkB^S#r90_tG0-%uHd@N6oEP8o*ae2W37^8Ds98GY?)*nv$zx>JKcubM-@GaNkJ5KqWb5vE%e={}^ zJx_??vgvWMbu#l7_D@lJ7&y<55N2`)a;nRa9TMP$l5 zlktRcDK_Wg@mxN)uUI&pGrx~WXm)C-Z$Nz!;#UxJZ}v|t6l7p4_RPr#i|Ox-5BGL7 zcMJsCYe*3HQ-_3-%VgVH0x{Ng#`Bo}!on;BSh~7=<#!CmtnTDv)WhfE^CNyMRqZOfMcYVpGH;{_4} zio`Ok~XIVgpd`=n=qtWN#Bajgpc-Gj~SLzT)!}K$Nz*PDA`i2=3a^KumokqSCjq#$~ zZ|TY#;>RdQ(&E$A+5W5+m52dIVB_ zNXr&Q;YM|9_4a|D!)95lh3p5yqYJ3FY_s*)1E|V7>Yg8s9u~)cF5ZWK-_W;pVwq^z z8V@{w?O(bhk!+pt#cr^Pr&h0?zB=Pb3hI!Z1^$mCM`(e-1KB(IgDT#yi=MXS6H-6I_RFlCt&5`0=n2`V!SHPy~9w)9@80 z(^}{mU43bWM4rcu{K6tV7?3zLV3tcc{Ik)3@2SuNg>NYvZM-Y&j88PWSj>J;!V_G= z;R!s#gXdt@#?mV2$|b=IGZRLc$vj5JgyWLyQ&N}@<(TGTiFw88xO-Qe+H)=&kPo;N zrB4i5uChs~8;vd)_~)LCb2d&^lL-nprv)>v>>QoHd7jiP)ldr#BGpI%JBPo$XRmEs zD{1Jt0FVR_2bg?I06UUfeA~Yd_VX2)bZG)P?(#b3F!Bd85X5`T7f+6_9WCZf=9&hc z?<$bQqYwE$ANvZsXUe{NDVfo574wJ;yG`g;~_SV+A+SWk=dxEm1Q;|PR zsSD?ZK}=;qY^GGS3H!irvHp9S13da{3Neyo-N*^M;J6SykbT^bpTj_q^%85di;|7K z&|e?LF4?Z=Pq}$Iox^t%ioO5SNbC#M`_ehM8gAn8D}!0eu^di{1F&&nY4h;x^<{3T zJIMh6HijlA8Nigg)GOC1NV0my3x={2`=`B-DR28Nf~^+57#9*^4>ts~VV@sD^o6 zP$`cY2~NjtL@iW@&IKRGWmd((rK52QKAOWa^IGGr7_E8i`1zT&Y?u zAl0&uWBu9&qK&R2L@~SP3D%391>=^{)u~sYm``3sqRKPKOl2fspxhvZC!s%N;2t+~ zt}pGfWI_ci353r@Kax-a8z8(CDWrhWn7Me%lC7wrf&iw9*>WlIv}%>HVapZkCW`X{ zf_ZDM8YX72rMy+Kxu*%spNo!JaG07|0)2cU>yrsqUhJ0%j_7%K3Xf^5r}o&~SCutv z{aZft(NioJ=AGn zqidPtkwr2h(R_ceoZ*u&n#zTx)~WEP0#jbjgd85;5|{GiGwqFPz!;!2O`|+U{>pb$ zoJ@rw;GK34`NGhXf&U&PKFHk+O_#>aor*N3IH&||SwFEt)+i_1%#xp>F3`5cDQgy! z%2AR&$#MKcdU^PJ2$Xv~daf%i*F@N3e88_y7w$3jSo4~He)e9oSpE?TZ_e#4qk`XY z+vQ$DpqR6({nh1@77-C@od6R2_BI%V`!fe*_KHp33v&j(js1%W(s%l03$O72bMO;L zC7aRqCG8720gH9(Z3AV&VEqQ%$56zUF345^h825Vi z;_{U`yoEWcc~UZx%~!A9yu6SL((!}YZn!fzC)XJ}^rU!WZsGFmO~vJk5hDlkx;(?2 z4PU(+M3)2gUNyK$6-IX}_WxPP!9@EwSociAC$r8sc|&E-6mwu6V15pmL^L^pVDHW7 z{}!19i|2_@{u?7WcYMS)oD_GvkL?Mn zcX(kvjF`A6&NpLiffpc+aHh2HeJMUm5QR6nz{N2pJC`Sa8j9d=zMlfHL~wCQ6}Y_n z2>|I8(3?XMKNYz?k1Yzgl8{+4l=TX4He@xs3Dr}oIFev)Y#RhIdu9FGZ(kg1cBE!P z5N2U{|9CO?PF7D|xM{?Kjy9zMuOa|wQl3B^rV^YiF9LTuvSB(W!6`yz@(3@y5iKF@ zKFt!~L;HZ{3PY3U;DMhU5fX$pcN@qADReGezA`{t9+<;eC;^f01d7gzZPQN_`Af8# zkvHb=A5HSrk!a}M`Z-lDr1`1n(bQPhY3EV`3J45OEa`Gu#xzqlnRP;K?@~%-_^0CV zpR4e^ckicY0MNi2Wj=soqI1arzE9EPrbEu;ZkKK3A}84EYSE-cRJE9rr8L=FO6pg! z@s^b{J1#383csCb5IYPI0shm+d71^#VK|HZiOem#whM=lqwR5n(1$C*Z$^_4$Qi#S zT9tMz7?6{C?$(2os;0>>y#jzpv22vz(n->=)O$4lma;WOaNrm<-fdkCo=?cxe0E?jP9$YHse zOzSOQS;lAtP}&0hq&(sSiW^8sIW~GjA1+D$Q6UXIKP%4HDmp~Mirh>o9MZ{dwmz&~`TpCNXZyTn;DK}B*}&4R zVHu{(pjloM0nB~?0(IbmpDHiH5hqeC|Bs83{mAZ6?l7RS3R-h`b{!U3C#hXDg>cmK z%nZrC)GnM&>B&-ANdr#Vy?jJ*=XisRUSM_Q0)Cll5ey{ERW0I%=Yxq#1T{bAfAl}VU9Ic!H$tnYaP}65Dk^1@7phrl&$Km@U3xC^f_U-FU0`(Qt?`xpsO6m99JBT<5qZgx6mD8WySHv zsnl|3`|Qj!+)ORm!3epp-8$dG#_+7}sS#YxKV`l+W9Kh+grsAM#mScV5Z3@vay!6) za{{l+5nevE3&}(4%NMa@>yU!Sfa}V95j7*Cu6U_nIN^2?E-J`UQ~hFY$%_nUapSR& zEI@5S-Eqr1D30czzqt$-77M7@UESZ}xT87o zv1CfqA-n2%mZz6Engsi_u&-xL1mI*ZFY|m|I*<4l_cu~J05s2oCsKZgxDT%#PvQRL z7!_wz&dm=gPWLh;5L)N|=gh4LZb3a(QjpiPkf6#H`_9?vY4WN}CT=Je&~FJWgKrOWR;54L@xd@5Ry z(2Hdx5_nQ}x734J3>NdGOMnu0DJ&$GTlKI?rSK-T{LPL}DrY1X9Z61^tl3?&ymA1308F92?0l zA)wJ5iL#8Dpv#5hgHYedcqA{3>!XVtJRBr^2KoAQDdswXeB?cA)cGO?<>Tk?&5b_> z-T8}ic z{t!XPHxkf#c#LGBzk5{hj;@WeRqZ+H-uRr1p6wF}Yo369<4ps+`R0f~Adht|Ltfy$ z4U8Gii=@IRlzKj)-m;o_CqrsaxZNl=Wr1MG`d3zgrw9OWp+zJh}HduPf-86bI8@BaXT_oQ5ZT*;5uQF1_-e<|%cq{=YE*>;M*&O6Y;3ma~x3U$MKVMDT9g@+CtXX zIl_0u$nsFb!r)+e z8QUlbQwWWsHzxU*oHD!?O3eAEPABq5V8LM@p?N$jAJcTk7YFt(NQLkL&RrU)`b)4M zkL#feDtbP{;CSca;hF2V|#3ux8gY-dS)S3LSKPUl3MV(9>hg^y`hsI;AS^kttt!ErGmRl=l zNAh}e@#9GmEA}S+uXOcfPZFoH2d}v}97$RDOBXxVmfKx{Ab*Inw<020x!;=D`TkqV9t*zYW zypiA#n=_^+Miig9#uRAeDe2^l4wEr?bjcD`F;vc^w&C!&cJxeyf6*Nwnb}f&ZYuGt zE6D%>A=roG0hPUCh~XrY`~O3HG2if>u#s()ss14ivaAuBBNFq=i{=I1a6ASUV?5hj|&;_ov^hVxo0$TEQS5OQ<(Kwq- z5|s0$I2^K)=48#AUY*x2iPM`|u%c~vNi7%lUlQ+f8n8}w+-$?@hcp#9P{vwNsMfkH%0PLI=^ znuY;!#wX`{7`-TYv4NRjkE#>tG*bTBH&6aKCic~3s6lPq*qOsG?5Gc47b7SkyH>uv z?Tk>Si>BiM83DAtu-h!2-J^j$$uM?Xj9_wXOvjTqCB&x})5`&^dE~H0u9V-0pMfZv zE27^69Yccl;TZ?yzMlpgvXo-?b*kH8n*%#|i+qm&#AtDw*$&6|2wg(ZadG%h;;7{( zfc*=h>QtM~;*7#9$DPhhD>nj z%D*{1ReJzSm0jTEt&{J5|Jy(K{qHZPdqfFf!i3`f&6phPj zJ>HIM0#mMPTTt>M3)EXMGnL4rnfULdFwRzzQ)yq9$2th%S)oeDcP9GoT)usc0y4wl zxhHO9;KOUPKVD$kT&z4FArsHYsd+f2!!d!0 z8BDT^53R!O0lbPOvG79;h5@JSJ)c=lhXU>X~p^fA~TZZI}tQ3OcXND057pQ4I9DO$%Lenmzuob zVMM8fCNq|J>%b)+J{b!ABxQ}jD*|{tC1uYaluadcY&)SX-JIr@FnzOhYWBk=q;nO%3Znm;8ZLVk~m!0 zOR~hBw3g3jqc234P{oWKmlCrhVQSMV+FY1<3i$M+dlg<#p-9eNPxg zT^{C#Z|omwSd~6d1kJ~vW)`5WhDGOQPycxOU9k&Mhyz7wRjt^b&5RcL#iAy9(>pbp z=?yCu{&r*v$bd-2<)B(2_7TBlup&$$q%{~M(tR1?85$VpYKG5?Fb1+?SV#Ynv@;Y&hpqt2$C1(=6isjuLKhRpr^V7^wy36zy z7Hf+<2G_65B*aKrN&9~2y38yPeVUqvffpkgfR+&}G5UJ@%K;dMHD(x33R_%|caJ3I zS^te*vw1oZgp(s*0>sSHOPKXH>WCxuq%>#(Z>2ms4c(!zAVD27TSt8Q8&f=VYY7Hn z@!`VAv6A)U=8^N`LUXbIJ@=*zlINGNKKtD--n@N|wF!uEM|ciT$U_B?`6a`BK)eLV zVs-ofw}7a`F+zD5a&i2|-fldeMqJS-sLOe{Vt%BoNFGALrZ}d}IPC;))CHt6d)C(E ztx+MOc>F=i0lv{1zL6*Lbev9pM?l8o7|iSAsy%*PG_mf?s08 zNjaTzA?O@nz`%MDlMN)>jq_2WDhBGZOmW=C(v>w$U=aF%e^s?hng}}b?ki$Ay*aUn| zaTX0VPYiAgn<^yb-nHW7ABCn%Y(067FW>-t(j-;;_#xOfS0{gq(5I3`YFM7|eNN}< z23#FVv-v1VzBcxacM_zvby|*GeG~6@jT6~BJvDz~Yku4w_ZCwgdq$=+^@2;QY(bmNoVmKd9a_uukJ1auwb2+&~1nKsS zqeyqpPOw+8CVj|fuK;eLvF5r4SmD}5_=3MAUusU zjxuS|xS-Jy1w?KhBLrRe)s!xC+Ns5q`PHY)z&G_U76AExU6{nZkA8)h?RLeFthyD{N+?RHrEeNVG5UAEKC$+0iMvhWh2&j0+}V!|#NQD9@l^VJ4QoGL zWbdjC$Z2NI>90VBXZx9m;ra()6vy9_9_PEINPLZX6M}nuu)j-mfi49YVnE3cg-V8o z=PUVP4OouJ7sNM_cW$9#aeVXm@apE(jVpggup<#3mpRYcDZ^%bd>6$iD4surZp4EN zY{AMiQ=?Rs;C8Vb6?eKT1z};EIM8{>BMj`;eiuhn%3)UrEq79``;li?i-uq^2SHhTLBGzLvGFO1nD@0 zZ4fD;ZBHi1cPoLOJH?|pa1XW(p;??RaeER@xJSx?bULM)x^^@wcyBptD+~;PL-;sl zJ|9bdS)7c;ivE7NI@CbGzy($=Y;lSrzpEfx$uIJmQ~ktc_#$L_>;S_MfHolOwhayQZfuY7h!9Bf4PrG!OX=eE$qr(bqT5jYAx0rk#T` zELJ2p=?k#+>T=wWpmL?O1p?juBcMRIYF;wSg?bGC#YM^smi{|xsyHhMwnYy@zgr7 zN^5E!-M@Y7;nVif`OSG_q3o<}X|>1@<)u4)+OO#Li={v3*4RPNKc-m#vb4gF*H#h` z>zMfa6pU0nFMQ@;2Ft!NWj;}e+|YUOT#&7A1`T+nigj&Rt5}sRa>cgk5aDHnvm^m# z-t9nbFidE4uN)pdKY+<|dpwiFi7phTkI&scyf`guLc2M(D7kf_O0n$`OCSC7$ax1BSKBYV^~bNqpz;MrE_p9 zzG(x5JHv2g2=U6axjM#3X486aV@+jcZJqdETT^+*s-g#X?r57DU)b_E`UX0M1h%!X zhABirg>VuQuWh|fTF6JI%YiNcwd=Oa%bjOh(o)IrQBhxKrxnL@!G1{)bvz!du?;E^ zf|qdlu@>XXek3S4X&MUorxTe_Z3~;A-;AYkUE4Byhao2jM+9}|Ca|_b5`ZH}w}gU_ zvtaFOyQi;Tlq-V%c`5{n(M)&k^d#nz(uHdo56w8}$(h4Ed*iUNyW?Zyqa!mB9(ZG- z&%r5IBo)^nRT>i}d4kArq!{{-gMIxOlSo^N@G+=Nb#bt4O=SS_l<2?=z2_{|6Ur@J z9>-;7I?)EadXYM;iB6uH|Gj+q_OlmsTi5_BttT3MOZ$KAl(x})LgtY!l+V6OIB?{A0KJaK{Gs4=BVKwc2&=U2F3i*>6v9r?BZ~n zHF((vSX*23=;pl#5ANN(`LL#Kba8jpMAebL&dx4e#>y5ajC56%U$^#Y(s_^!?7fi& zzCV6&)|hH>-hKCL3c1M8w7!MaR{l6Xsu27)%f#x~4tR6}wF-0}5f6>k_r;WSSB57!8}<+TGRBIV3yKlrTU4iwlQSBmMdSN}Yk| z!oh@dN`1m1%d^koKaAM4%6xok-tcWk6wzu5NTmwFH*Ng`4j`m&2y{Ug^Aj1D5cl5w zyC?bq9EN9l_WC^fXt+~O6{b%g zJ2@Li9zCgR?C5B+*{Q9wXDkkKRf00o->D07cDS>lrFV=#vZt}8s?O?HL;iYf!0sO( z&XTujRWf_ymuP6`=SP|!*L9m}xVEz_4)VM%+vf7>3Da^lm?jI&#;WPjp3e5po_^yn zBI3e9{a(n)r0dR1yv{$HWy+$QL5S~|ROfuN`K)eiMCTH;)awz-9)dxUMqJBb@R>&` z7;Kgmm(r+19JueEk19|Wn#v7M-oBFJa?hk)O2Munsg`Qs(v^%Z%-LHBNH7-2BCFUr ztk`*#gsAMXbZ_`|#1T_~?8Zh6!|&?uQSrwsVwZk#@!wgSIiZVy_e1=ri3~-Q@q<#Y zpR-sA5AqCZm}PHmSpwqY@L6J4@-hx5zyHMYsk+8?2GC+6 zasL!&xxWLtjt!;@dVaJABFM*$(Kk{o{|pvlUn{BA80!>O_N>t)w1z^zS7oU_kFPp? zS2-y?>1BF!QGxM~#q)Qtpxo#jL-hKkkv=a^!<>S6&p&&fjwvA^W!SPXFB95Z7dQ3p z?twhiPqW0y6Q;CCUI%*G8(W%ev97dfuCk`m+?&>(Zp&VpT048q1;xjBONN>B4vsI3 z_qBCXz|2kj{(u2H4bAq`@Bw}BtP%Ux3Qve>69)L2jl8xp*8KQcubm6qM_YQvPX#De zEG`57#J$DEU!?RM$~fbCCucNuAn|$ae2Sc=`y1Uyi3m9iP~N*qB4`M-b}Ct0IROHc zgyMG;ZZ%XuI*~S^dpr#Epn?)CDcXE{+uVF%pJAnx2m$bpDMxQ_oqzj{4MV|V>NF7$ zYW!L(lfWq@ex4Xp#&+@sl0%%lNF~YDHwq5wdqctd@v-xR;&H#4PQrBxLd z9ph~}Yf;^-3OOV?$hvz;=@tY-+La+0tE&qWW1|z=$hN~5%$V@KQn7p)APPT6*D^Pk zQlr{iNsI)|K(#@A-sQ&c{=Dws1}JSu!nzSXA}`=+3Q0=9>+5q9{mK?yZ7r=0PpY2P z7S+2}IvLzQFg!8N4{EILHQc>>cyS(1c}M-QFxtgKn@v7EGCCoK8NmQ7P7NFQ*gc?_ zF$>dLRxmI+JsWi;Dr_E0*4x%OVR@XNIEVJ0%uaAU)wakyHCd?zZ*H>v{M2wqhl#;Y z9zVKy{q}>$wPyCsTii>V5o&aOYceAsi2?o+4eY+tyKY@h? zjWmM!v+lf4EQE$di(`?0O4p%w6C}iJgEu1&Nyg+pz&k16`sRJ(7LF1qO?4U`8oD}a zAlz8wjiWfx6P_4=PGmhec~O#~?>B|yl1p;|>2J$&WdlhiCsm4UzxlZhkj1N*hwD}B z+r`Ul!u^YO3SOX*z~EH>;_$t5Q$;d$dsRK$k4l5O1m_)bp_d3;lp@lACnksj3ZbsgV&?cDHlWWzVG(CcZ0_D!Mv;N`+dFHBE1l`qxwO_jGs3;8siU{IZ$R+V zI<9{_JMK{1BW%E-kB$HF;`+4xb3T)88oJsdo|`w`Z(_i>&wxI;0Q%dnblJ-SWR2i zWfm6hyq2g?Zj)WuTaZT^_s#$pfrblBV@sKo&JTE^Cc!B2)A{TrGj~+lc625!j?NZS z-!%>Pn^YncJGuix-eC(S$l6@?XVxw)cQa$+25(PfF3UEMGr`i2`A8!-$qyUuxu z9t-(~`VG$+m|mJfT)R8tnRu3VTICFPe+b6!8wP*W3&%#dj)Cz>Lp9R#NzqA z!}Y9wonIKYldG8hF(_oERA*~TYcCWsZ#2y2vhnJ*=*M*3>hOhbqV^`LlmG!{S>H%8S%GQun zK+~vT@J(h}1QfV@yZgY|k;%CyYx4!pJ{8dS=PzA!GY#Mf4iP$F*d0}HQ%tr$hd5IC z(Z?H6Js^^|WLuEI3`I{aqbg8@scYxCys+hX?2*z-G&o;J5OH`M!|>Z<#9 z@87+7>&`<l3fitcIFYQE*;Y^HoqSz zl*_ldDtc2{&5ZXNH!U^sdLjBVk140Ww(K3ci=GZ0_=>E-nFq@gWTvL?!5h%!$rs}n z$=+ynGuv49rFYPy<4TDMw}BY;tC_qQzB+nk%1bgP95jOhASL*RyE>GzBmJkjyNL^g z`P@yZ?{fhYS_A_+vuN8KZcM-wTTHT;Bfw>hfP*dOUds%aK%%%m5~$$VGvC$rS%Tfn z#0}DwI9fvdj&=KjaWcXf!(w7ChzkKsLC}75?2t!`2?S9~x4cgsq`R6|#bYzuyPK+N z>+7CAx^wsO)0(Hxp4HdZ)I6=NetfU;SxfimRCbJ=QJ=gxQqVp$pwjeV0oK}93U?D0 zw9=?;N!hlRGxKlE8&SyFdM7VJq=n9w!mP}UstsggTvg@Mmagu$mS$Unn;NPfRX)6T z@4=I*Cr=(exclJA!$9|(5+8zJ*e(d7XzEK=-0I4vdo*5*G=}X zopqpCjF_LaiL-rhn$Bx`Z_c4XT1f{CDClUqy)FAC;J_ zSIBd-m`I&gV-Qbq_?)_B&rZpisbajCygEf7e0WVv8Qa4(|AaIygSO9}D@~*!EhQ#- zx0|o#!WSp11oNkWns_gOJAa{yi5efrExd1-bp9}cmq&#}m7nux>@jtmNys=`Ju>4{ zste#4HM9JJDPtzgBFIY9qzG!ifdNt~HUt|xbw;ce*mTl=m{}VHgNTBCb#BYmvYxq9 zbuZrbQEgrAv-gwodYpAJ_Z9b|g>i)8- z(xTh@59@oe5xuRAHTIl8s=R;u=8anqs=CGi>ZEoAlBEad;#@j^Aq5Y2BI7x<25YRp zt7~X7bYEmHs;_c)!B>0Axq7l_ykf=HG6=r|wyVTtT$w>_XIMMyM)~jDu*C0` zq!8#*1(aRZU=!0=0R&trMWKR8;6!e^pF}!;I7zrI$$?D8)Q?ci@D#rzw%CXlSvh0nyT!inW-gSa5ZIE1+3~curqIY1 zmW!1gRD!#n4s_Twe4lQ}^sr@Lg|a<1P1sZ_k%Uf9_B2#gw~sB%jdip%G}Twz-&_3% zYyaT>!^%gGD<9W1nA|@&1nsF&4D|Mkee=tc^Xp!CehJ%4O7Y$t*D~B^Zib|NdUSN0 z_n)T=ni@4XXKGdiYF zri6F{{k;RUc-Rax(MQKfG8R2wbw=WXFtxcei`N#=+>7Qn2&XB-hl8KAmu?&QQO%d{ z`v-wh)_FJweQWZ9jCQt&V_kJby>@c?>h#r_E92aJMJA9!9Pz0K0Rifcaxmig+03Mu zpJa!^{K1KR4#aT6GAPOd!p@`&O=tOY3&=;%SjY@B$s|$1#YJKBB&9xur72SJEE$r& zgwSCZCSGz-0hykFQx`3w*^u3&<`fISH_QBGn$c5>QXnCgK=b#%TDQKdueq+Sb9`}r zqPxA-O3->|QdL=1U6sW&bs~V-+5ID9fW0nQ>5KZn#^#(|3koaC(<`PyZxN8;S9i)Z zX93WfF*;X{n-w3DD_Jd+kxB|x^WyYyZ(BoAGxxMsglTSXYpE}u{!z8-|M=0pdv|Z& zynV~Md!+G`I+oYeP+j@x(c_vYm2m-g9-6*8u`<@*-Ot8^2^4)r! z&xjK8Bd`<-2Yf)8;PCQ3dbIE93g3yP!5&Q>@BRdynBg8|Op?k|5VfFX6} z{Qceoto{kW8On?Y^P-O5Tt65oT&Xyt>yrlvqYgkwN{8B?J?oxaT$r-|puMTC!DKov zx5{ey+UlC3>fJL-3Ht^;3VL@nYCV)Y```y4vub5KN==nC&7s>MiZ>99DjA>|PvHc8 z%4Lj>mJ=4qte5A;20APZcKGT;7DkHwO${j)aQk;S`P+Bz-oADB&aHdZ&28PyO;6tf z1XVeIV>fTyc~sjnvNUR~g!Aw08gW|-HbSvo9eu?e~kS}Xsso?Fw2BprKe8-SKX zX?pG#=C6$Bq8nWOFLJWEs6qbd)l`Oa3qv@}k9LcCZ4J%s{Ug*KsYayIEqEEfQ9?4& zgApYt$I;|Hh8cF(V8ZN|Sa5orgct!k7MA`AUjEm_U|vn}0p-ky%{s9UeMbGxTi)ER z*!jbR55}*x*ISAD-B?U7c(6V_WFme;OMUa8nK<@EO=QRG06J!L0PMz{sl^OJjZtvx+$ zPaod>^phK(e)NrxKDl-G!6Vtov%1YXHGSA}rOqyR_k_}AQVR|^B>Nt3yW=EcT8hUg@00yZWJ9kn#kJ2=qW(PTfn z{qR9B+9%4Ck&BRslE9_kM`UpR*iV1NX(=GfSW}Rc&Xr{*X?n_KK7~CEWoBUcb+U+L zk3J82aj|0MhwB+i4RMCq1o{$}_!(ZKL_K~I)7zRI>1}UmY;9;BHs44XRn}C`d8=DwHJ?!7n0$o-3gQmK&J#Q10WI5r_%|B4OzToDV}tZYGOWt-dmb5LMbefAFXX0nB? zk}t@G6-Kw#4F`Oe^aGWigqqg}`{Srf_nwwP^pR`3GCgSZwCQ8L7ELio6t=ddATtS} z%c$)ufhGTcuuJTol7;#b&p|QC`}+>^4u$-3--w%!Z_4ZQUu#L_<&UTf5uiN(e`w{CxWua41mH#~U= z3f%bUYad>}1&~%gseSgW_EA-BV{?5?V^as8GCA6B-w=nRv?aA&U(8^&z9BtWqxMWd zbMqW64pj#&}))jAZ<0Kb>77iW}5@&{M`2lYR(fEfc z(25B^K`rqw0!s*tlBCeVrSM}(Umv3!og=?_m#zm$$tGY83DihK1BRXS**M=jkhN)8 zys_bau)7{!udaM>{gZEec>9z3`I(MK)miR{C$!zi%~L+&hb7qW!4;m!Ooy?d$;I{M zNk5>i{*lzaRut@NGfKT{aG;Pa9fV>Z>bj8lKtYQunN=f7h&7ZPRFLdwVO#ZStPSBiNX@rdLgeZ4@{e>I1u?% z#==J~Eg)p{L3(C6t@SB>dC+ZHu(af#0`5vEh8fOfgOf0*!`)r#)6}R*$;3k=RPn!R z^Q{ffo>bnr_2DNEZnv&3^{V@`0a<)kZ-1ZFYR2iqgGl(Te+z_YMIx!5ot!uEZGK{? ztF!4z<-LdXtrlJNw6&RltU%h)RP*@JLz&*gAc5;QZryrV{mh=8c3gx7iBGC2NowmA zpoNa+w$9GhXVocCU?^IP_IKTVU2R^uT`nQKO4%HDX#=o49}oy96(YHxnfHqby>f-= znxSV1Tqy8=Qpga4;mABipe+)ROpYj}yrUK{6J{!rat%>`_)ZLTvi=F3O55eEh(9Q) z2$xMFs2H%nnQ>9^F(QruiTUvg(Z2s3oGPbDvUcYc>DEpab%xX zz9f9gFXkYDN{6ZQ{Zb=hFhH^yqtJT)wHTL-DWENety-m7{Nq%^&z=`rS;-o1Yw?UE8shb*Q_mx$4oq zhjrjkZ>KrvePg77u>lO=!^-TykDy(EJFX>0B3?P~Kr6%d8kjN)3DjZWW9bAuIiaF2yS@XLW-NsGeA6+01+ z(ubE%P1cU>=MRe2OAkCIU|6l;ZP=cqgjj#o3_PIgPrAmn78~<<{wd6$a;O8gu9&A| z3r$i$P%c}{OoPxe4+Ru! z(N7`F(m9|L)eDO$)|kd;x<43FuZ4pS7k*N%%pPR_f~%b=gTwS&1N}WF;)R}TZ7J&4 z>pGTq=dDJl@9Z~RcWQice54rrf2N{kz^?PK%htCJceS?m4zHcW0cnHm2;SjnY0x+bCKuVN6JJr?E2ja-{%c+zgx`?1G zEN4saciL|pgz@qcg>=J8Y>U-&0Lb^yAH~FexEbk!5l~i{`Q^gaVFrF>ea_XrtA|%+ zB2=VISC%rHMLl9MftZoS3iLp^#W`X}>wihRE2!tg33Dj#O(5uAWKW=9EH-r1MDu4S z1p!x*mG^SQc&cLKC}r$8AvvU@H6SI#La?MNwhax|DF6~818ar+B!N7iuToMH443(Kj}YbQ6NP~X4RQYlSlbjbpgfDj13Qfp zRr4g!EEZjP*Rtl|KKC`XxhVBPx8ymc0F+FBesTN6h1<%^_N%8?FLa1eVVTr$T%7z= zh)svB8n29tk8tbZMg|Zv+KCv92*FE7QxuB&3oQ_eM!bmrV{dK6`vP9yXYYn~q}V*^ zY!cpo#&Y8W1Q@y#=Z&Ejcj5(DMM``@2HSg3h$NKHkq5~DSU@@g2!xCC=Y;j^(qkTdl<-rAUDj!-xNrQUfoLjA)!Hy?Q?R z+qZ9i^5Iv%`N4l$%>e=jytvI5#?8kno*iFt11!w@nH;G%7! zb!dzu2gC$;vNAiEn!c6J$ax4lb;v-qmzbX_$i?~NW-?EjckNM$*r2rx8I851CUCnQm(*&_`h*vW71hY8A*>Hs_kYJ}m! z#_rh#M-J7~dK~SYAt%uUkx3JU69)QE`hN7lY-u==qz0H?+x^^oFgs71(DRoW;}r5K z!ub>Xpuqros0dXhFW5#<#d^jpaxt>bMm)6W=*WPB9#ih)22%(p@$2j9>g??9Arcwv zbkN;;<77zuoPQyxHy6kIJ6a4#F(;*~ZzI|HENStjX=*vHk^C3kj`xt*5=#K9tQH8G^4!~bIEbFpPl&+_HV=UFqvNyJGp zjuTe_<4DHt@=KXYTM%cosYS_58K$T!bLsDp=ueIpQ-6pPQfleL6?8g=)}BF$TzzA6 zmfGa_y-86>xoUN01X)jE(w6lLpfrM=HI0sq4)zos-)rx*o3mY3+4!{jo{Rs@5Ai@B ze*Np;bn7dha_o<9UdIdFyL-Q~x*i|hFHjUsrxb$H&Nj8<_VyOEgrk2#rka1%Hc^lF z75)EFS8irCl(CHJ5fdW|p_AKM_r5r05tC1L!6T1D3za1^vk3G;x}@wOp&(y&Weh($ zJTN4P7(4MlodD%nu`ou$WspJ|Ry7KoUfC+=NzmDf%EB<`pI#l4+ov<7#Q-y7GC1on zz_Ed1Z(ioFW~_$ny6*pbGVOFXYDT6spr-02AaAOzsud-NhWfi&yM}175Vb4BvE|S} z<5Kj^<@I(Bq6N0~ClBz0%0a_5FbnE7gZweY8rA{CTsHzQVz+kEx z+j|CwHKtkk#|v23I7?(JvMmGfvvbQbL8t(?QY=lG;kvn`YVrHrI3`S+qEf2Z$ty%t zvZcm~*sup0l7Pm`I_u)RLF4ceV@UN-oiH__tK)P*`jc|895Dw|>`mtQjqR9GiQ|zC zucK9vCzLM0nWwav(g6+WVa4%}^3BiBPY=rlcC*u?>Y2k`4Mh0egLs*dzF~XAQt#11 zV`o6RKXvx91AVXNsc8Sz3~1M5lvgI#_m3|tDX_sEh(verCE5K!h$9ti%hOg#jva~NFS}1cmLLXr=PuxbTroHOpnlo5BO$9`oW6+w|#>HZA4)V;O@i9T9>(6 z`-vDn)Zg+9EAUvo$8aSceW=IP|Ht5_)c+Z&QuPy*!QJa0+_-b6gojl!fT!vS_a9U~ z1p`O5;YZ5FMbC_xyUszw+DKVbMmb%mcxk0#?SQDl@PK?&{yYT$GB)W_S<&ZM$pga% zlGQ-xf#9h&a3vJ*Nz5`MV!4=~bZ_S7uFet%Pw2q9D9)EfU$xgzZsFbxTcdb43L$P< z8%EAQa6P*{|4`FvIvj)E{1FLS&Oc%=S-EcG(J|Ws8@u{P=h0ZBQ+TB;S27Le#7tOm z#(ag-=f(N=j07?tNdE_?zU3+t==Tp!t?fYpbp8s{#`oI@r;}S9OYs+vwNdQ-Wpcvv zmqyg;VaaXJnikY$L@WYksbP&O5fdtPg8N78PbCfRrT!fr?8|bLevV*hh(!nn9UQWF z(AwTTq!=)xHKhLGo%@fQy9SV6*zscNKV$$dH=;FmV2Iv0t62M5>y&xP0ORK$h4Q(N z{ERyZ%N)5s+6Z^8mbP6*YY_fBsIV6i`wpCaR80c(DrkEdSF zP$ZIEQ1BoNT@U9@^=deONWR$n+lHqxkj;6tdC+!3>{F7A#}VZ`x_9HF8xN}L>ua6@ z$W0yNs}u%+M5s%Aaj|@)Hn6&Gju~k_=*1m(^z^rP^bAgvno<)u#T^u3vHBenLz!nb zpe-%q8#5V2FM_->mS_fXHY+3(UvdAsvlJWV%>fHBg32mJ@6P48q?#zmWBs?;{Y|#&b+v{&VxTi z>}e+J9T*j_QCk9Y)9LjOc#it^0 zzUBF&W9(I&HS?EhNNlb+{FjDvdH9=i6W;hRw>r?@+oEEQ`=rmRynEwbbzO5~&C`1{ zv|W?y2WO@wYl(nhcnmVJ!{eR9O=Eg+6(gKxk3F}2y@O-3p@`86uO*6(6(~f_h4)&? z>T(^W`d-Ca;e3BvHcPR-~$xPRRyAZ9hsQG2MzQQgNXp6%SSKY>`hSzU7I~x z1QMZ>rn=g?y84=DS+cEbM$d%*&QeSMiG~1Ok>nsg6Op-Y!x3R*5Q1!0RVMrd;2K+7 zTH3k?Rh`uL6pQPdn%jEpOLBOOLHW48p7Sqix1Z`6c|;sc9enc^@z^aY0fpj<>K8Rw z00AQfsi`q(hImfBHnjxv*$Kgzoto{P#p)TZ3&#NxWCC9z__BuVu|eU#tl?p4iuJc? z22U+0^Soq5o>hwNKr+V|t}Hhw&nMq@b($)$;&h7s)BQ#-hi%Q-XEK+LX1&>~AfOqF z$=u*It^rjzXR4!|ESjO>z#`BSS;KCIDDar$!=wYv&z@D(oq8IzmG>&^Tf2~;Pih-G z24*%6b$()XGwStlx7aw`Jv}$7(BegPkYQfBhYH36pIJuy;Qud0qJvP-uC*KwwQ7yp{&x%Jz9=X+ zI{c?~RZm;G`v%D9CLFHm$bO-U2q;INJT)fj_lCQ{C-)JCG}h1@-j(h@s;NTgwJ}$0*_r}fJcPpP%J+7+40I(v+ zv43nL?Zz2k;P|JO-=99{79IGSF#fx2Kte!f7a2G@Xn`QmhL(w0y)=kZ#4yfAbrmEA z3KHozE7mipHu{m!_8BNX?$^cgL{x<)Mb2L-cGs{-soIOV@0s@FgKN7hLHfC)`$t)p zkfr9u(R3;K$vW>JD$!vHcDK2dD10!Z}e zE1&UzP0j6t#l%Jihj@3q_>yI^gIz7P_wN+G|8wzWwsd?5Qw7sPl@som4;dEz$MCdUzk6d1ZfVajF>m zBfXzfi%VkzG~M+25>!Esge{gUZCWH%xcpZTyn(1FxJ`|v=oUgM`&`^$$yrt+T~8dAm>JCpwT(!oO8~NMk8Y|X=fvs+@(ZH6v@?E z)JiLovO-BL*|H^d!=#>W(H_{!*BB$RGhM+RU0flq)2Wl zgnR)X86ePEhb>Sn@)P>ca6td3;V`%Kt>rom;FDRKGEe8jO|QLLh=2tG#=i^_n<+t^ z0g<7K266v*BWd)>1{}U7#6}2bT+%YijW*2c!emFY>gW3PtCv5#aQWKBPcD3N`NqA5 zcC_s7o`ESa_R^mK<|7LO4cOjv`W+7qZ`@Zr@r}6-4Ud&wQ*+yQ_49xQ|3-sFl8I5p1P1Vm=~IqEe@L>s*UbWOixmh>1>AlC6({& zSFgVww`EzP?|VramM1hV86Q5xdg9i29`ptKzC5|(Agp- ziUz@lU_(BX96+W3mVJUO%hCi}`=}udHVBQkH$70_%5FY@hdTUAS8m>Gz|U;~r%iLG z>Q1%^YIFQ#DOTo2Is{UbNss3KpEftQx3{zpK<<|}ciET`6gpvWC}e}veVuWrr|LFY z$4drbaPI8#e<>9(5}Y_k=!%6@68Iweg z>2vGZXJab^2-`#Ijv+yT7h=l*Ha3^i-ojI6uCjotX_*2~&jF{J^4q^Z$^ z_QlXbEiktwJ6&6t80v1lef9i@-}~-&zxTn1A5{#=YxR#>x=^pihRfhWU+?g=%XYww zch=`7dm3+Ey>a#O`R{$_gHNtruU&g{wH}qbwb|npYX@*DW`MJL(ivHMaboUspLA^6 z6bCpy)Q8r#%#FxUv@22y7l_JUjdb=o#Ja|yALv-u*t=oEqdrSdugvkpdj~W}z4?QZ6|GKp`x^sS{I_>=Z29aH~lbox-%b^!T$4K6KA2E--riJ_Q~gwMM2ykMRHz%jSV_ z3nU4A;wxo;O3JnM$_*&?68K{RtN+~ulytk592+oE9Du+^JH_^u6p9)ziqGGgfRelH zeJQIvcxFm-8>?+;dbGc>{>p{(AAfxQlk?{z6)!|2!&Hy;ykzgXVGWMxRQ1kbkzBOw)-Qdhz1z3*ctPMuF6aM-+ch zr`EPi2BrElVw8}_&ZE*~P}6ldM7i@`RXlp2h80c2DhOt3v(Kj{40;at3*xE$WdPG~ zL3A>Y(Zep2D-Lr=CeVwt^DAC@7?M8+QTA2+{k;PUL4%(-oF>AB0YyN_g%jY02?eK- zJPl{zqo1x|6RZgOXDFbdJS>fKhSh4F^UMY%?ANVr5Q6S*h_m`+w6CWdY&foI?7K`y zUW;l!R>*wDa5Ig~iSi6N+{y-m6ORT>|x$0Z8?e>InqUS<5w;hJD_@B+dUORY90u>Ep)*#93%Who1J7 zsHuSo;=fZPeGRuSo&WfQ_dopLd*A!;{Dlh_FW+uxN;hpnqx;{Ru61vbZn^XAB_ZGu zBGsL1=imPri9&IF^M*=TU$mpwx{|5U!SV6g=mnS*SA~4~B5bGtPny*y_T)y4)R>ob zLun|15tHZ!-B2K{p2Iy!>)X?*t-KZ(6vUCe(eKw2R?K&|w^a@xTed;`W%|FkQx;xn z#Yp|+fJw^wwF5P1poe_OV6UQ~t+OKzK%r9we#CU)Kzmo|J~8InDh6l4g$SrEf*T6r z?b~ETnE4up!$7&P5%YxJbihl3Gup%uZ zCKb^}5|5hXaPH9DLd+BjBm`W*Qdv7CiZ1}9Hp=zK*ziCUe(8;&)>tx)ib;#cQz8%v z`zgoi9O=L~+nhOh?kx4II%=sYG<5&>#NWVtPMjkS09^fEYep8Hj;$YnUT&M%%KT_| z%cJ}CDu4@@E?vHSNh{>$?K=-23xfdi?wLhB`^}{(pi5tO)BU>|rjH+mQGfGBx&9Yd zFU4V8zj^OrQ`5bB%~HaVgzyEk8=FLx=J6_mk)}xAqN!1K4hZAgxHl01#v#CE zz65Aqviukivdl{BCMU}1!O%c23Kyve4IXVE!F_0Op!EMobH44J-QAd>yPv&W2LXif z5pke_9u>SKVbq}-w_l#sL1HZPg28BCYhz3I$oTkxrm*QiSmIcAZwfA2%=o9R9c|(J zG+>I|ycts1gIdEA?!T$Y_|QXDe}^OiOp;onyQ_51_hKlQk$)IZqKeQ52;FURdQ3A5 znIoF5L{_43haW}S*tTv6fkH4`8;D1woKSDV5`hI^G_JX);gb7vE*TH^Tzu zZ5n|)b-TfPmH59k0hG{}RtT<-@MpI>m zW(P)l+c6++sB=umUA=ZIcHh$8Tv|?Adq`;^_G~L#qcepS*VrOIyel-m2w^T2@|Eki zZ$FS5J$ht*!+m+!+^aDTruxRkN?|MQ#1Bi30`ARP(u(+!f%AYr3gBw4+dCyR!OS_o z00Ks2SP>%--IK2e(nlMXW(3k1t2kaVJ%64!{`%c6l?Y8gR^XHL3;X&;{$zY&2_S&b zuoQWL!1S=8(QW_$oM9{mMUHB6T$q-~jSmm?_G$TcLzcVnOE)4Zk+Fzrc5&X4H_u|{m` ziNW@!ww|%+X%i#8-Gd`#bZN?Px_Y4u?n(l?JIMw#H}d!9LhB*DU-S7*=^5dJ2m!bu ztu56b6aw1H3QEG7>`r$AhFE6T!|nIl7xb(a%j(N0EXm-r%aMqVVd1c+GFW+(QCdX1 z^@Rv<(FR)&O(#vUz$fKkNxJTq(254FqTIR8MyqE+0okov$2DL?0018^Ixx}L#mw`W z;o-;CgNXolvPB19p1w<;A1KJQ@QsPLt zmArmkDokq;_pM`a0*`-IN;{MLC=u(17Hft*G;h`P_~G4q4JxGeF88Eo07s6DwM46O z)?W>KJn}f0NT%bTfA;zmagpQCRuXqWYGi+^t1mSwTi@RNCOKeMtB2>OAY^v^9yI)> zZ_t4UZ4#~Z0THv*9=QJ9V{7~C(mm%t3HqNlrIBtyv{FN-g2qsXot>2@49gu2cOV#{ z7L8$>4MU1X2+4M#{sRkifA>46MyPXnfO|T9A|X<6Y5o~2cyzx(uW)Dxr7ZH3+Ch&p zXBd8&p(5!dwB1#SoN$bPo&{O6S(Oa_ixmmPMPTW+sU4cCVk;`BOXA z)sj6lT61%wL;9)Cl_NxwYVIhVThtchMoswgkhj6nGQF%Xj>|JPQX76XJ~KtSaaki< z_&<65<)<%As*69{8Cg%0tl-ck=1B_#1mVC(mZ_HP$)p!*^8oYFFce10@N?~k$iKmC zq;|}9Fo7HGXK+ThOy!7N$f8Apg|c?y#@WRnnrNGBY>Grs8Ua98pv=CUH*Yqn_V?x) z>Zr0o6l|%$qHf7SXK7#036mMc0&DLmU8Um<4p4hUi~17;bPB`;Lj%3(F^PB?r<1KIaj&B@_KFS#~Oks;n7f_d%C`it2QzwIe-V42~-3`{1yV3^F2Cw&1YLqCI*Pw z*h1Nroy)AJ2k_-GPmbO@wl;D-Y`+NFSC?NT$f9rJoSeLT-(UIyNdojQm)}O5y&Qk& zyUZrZzkFh+s6M=e4Bw2PIo~bU{>fDXkyo!t15q7ZewpB-kfA&K$7YcB<|dFGMj86p zAVr0?CUbRMgOa?_7`>UTX6#9M_Gm6YS)Q3P@l#h}a&FoL*pe{`cKz(lmo~ETa1~iG zo5ivl3jhKEO9~rzXkdW}2%Ea%i*v>-XR!H@GQZ(xff6HQqhq=M68;E1bh&$d0|QfA z$J+o{@FCXM6p0v_r(zu7=IoUetE)j^gD(3XDs!9?85;~)L>X0Csv3J}61L~e+W^5P zLLLX}hje0mv~mpp3G{$Oz}pd^>xO(ayZ=5f@0kUr9WgcOZKJVF>OA{ zj_SVBM~OS|_$t(&x}lt5Z+T)I3XgLN>DN?F%TD|E+5e*>!v$1%p9j1a4dFG&!@^8J zb*Bg00_WYlbLaNm=;7Qo6?p|c^5!jl0PPlS^QQJeHm)?I{X&4pGn1D0^dMHb2knTG z?nw7Q%J1QE$2DZ~wz<~vhUpl=R;#HCwf4rwMsPcErUHm=I4NYA5`D;|xe5M(!gtT@ z{6kV~(%xuJMX7d!Vkc4-@RxfTaA=q!ntLOI20k;RL-M%~8DvfeA!Ixo&)|YpF|kaJ z4nJOAsTyZT(`DwfIe39m4d;RX!0?}#6i?&}y&;fx8X9n}+(2pnJPpxCaiGN{ao%XV zxHB=mI6Xc=MXIwMwKy)IrM=ejZxkHelE+?P0A$Kshy`648Qs`f0teBh^P~{0>LpbT ztv%zjGQ^oBDM1RK|7M>;09FKjIlX%~IJ|47@XS{2Rc9}>^WkCtJ*~3?5SHSUfoy5| zxZ4olCmXP6dj*35(zO>Bp2i;pDh^B#lAyG5pBsj+s6*EfKgNe~W4%kYEf&lJb$x9;3EE7QOocD6O^nNmX-U*}5IrD$J3)2WHt=;_daYtsSw;UIel$HUo~Nl&Rq zz$iU`BA>>*(;=6_aoP#Kn&nxPOnhWl=~MCAj46aAUCpAU2a+Bka4;@`j?Uhj$~27I zkPK_<2%qaLbZQeZfc=>FLP7-B_Sc`qJqzUoEal{QHA5OaFk2wT@o_ zO9mrJnZ~6&)3wjI725SNGJt(wD0cll{N$~BQ6(Z%57mGL)7%XFB}8Rgg(;n~=V2sS zuZF0erp~j?@^pnwIDW#C_t{WD*4I42V4;~ z+kFt>r;x9Y*4*REW;a{cPnoAu@KGuHqR zFfueUxv8es2IVKi3sk|=H~L;1X+)!)}DO3BzJk~rpWsi z96(2R>HVgEXd?R!n3kN6QUSTRv&0YHPX08bhcW;y7;Rz|S|BBZL<-CadT>eH277yO ze_ZI6wlsU%8MQf3Z;g*n`A0dfr(O8QCSA=uCxdW1isvu-F}uX?4z0%X0MoMwyL1)e=xdU7nAdy25Tcr5HcBLKDP zDAn{@JkLx4JgZv(5-AOL-h$f2`9m9=W+(bv8=E;oq4o^nJN5XadIZeNvtKJMWtPw+ zor$1}V&dXxN4*|r#81?04|G=_cc6htcL$niy%>z#l0|hkXjV#YBT>x zSw6_251|0SXT-mWi7+A6(?d{rDdS`H^TUTty6o>2XfT4x>TAjT zBTTbVHKzjyx)~A1NH=tY!sR=67Cbu)#;x3IaNOXwBT?#r#G_&)-!F@M%WJ#Q>2$%J z@v4GwNqeET`9u3NAa1B7G7%4c+0oCt#Y1E0D=7lx0a{E0{gEV;#h&Z^hFf7;IKZyC zPk|jz2O?Ae56Tr&fQlC=0~iJIe-%F9b0vOIC$ApGCqoPiBh#r56Z6{()w*k9@<^<`30BQHq_!dt#5@+EaGQ@4d_SNukh1zQ4YBN(m$ZfQQFM&!_5!w-AxgbvKHjg z0vDCjx3?LW)d+j^@ScoVb~~)*?(28>Q7l^BILNFfe$CjZ(;i{--9}}`%ghb}5BAlx zqEB2f%MU{O6!hBVX;ox;eEQygdwS9bUrMuA3d|I@%_!qdxdfb&;r`uhOji%{4O{@w zD1+)Dn~qTa#rv3o`~DF1ObGyz7@-C_tW}ZL3b*g!qM3shK?tN;qCFRgRYf7scFQ)f zDK@wSO;D|xNfE0|AF9^T59f@Dy1;|VN~Cen&d}ajY5-1B@lFBrWiEx~q6?JK|C~`Z zT64V}00XHn1@rg-G&1quK#2H-4z3xrxVAh!KGYGh!F!J$we^uaL5(~-ve_-pA7R*? z%uHlTl%xabpCpHJ-g>u?p7inM@r1v8?DW}t&v}5^LtBlX>KGhQ{Ye|p*01^D-oCM^ zCAy#iflmKC66mT)S3gD|6;KeQZrsnX<`Ht{&hnU1>xQc1aqsSZsP97suxRk`Az<(3 z?RuqP!^6k!LFxKq{*iUVGyvQ(X{ogOD_G0G@Q64c7cjX54tVIp`-e6q3fEfObIxt0eD^hoT(A z@t|`Zm`P)j4s?~CefwiNB=;XSGs zj?N;?&LoLHJ%3hvKJ(%|lVEkfFiM_W3Dmo@KH6=z;mQZ!{U1MR>LV;9J`=>(HUck* zv3tAOwW<%eYZt(Z=kjhP)3;MtaSv`>KL622m#$rx>{+U!qw}S60mz6pJ9L7+Y5UrEdpcrV%?nG2E5l?%)jl zBe2?-%*qy{Pb;zw5vGN7jWD(i*mni}SOqL+i*}wdW@T5~Q>e z_5}oP9{eW;i1x=B1wO9(%gO@udtbge-k%@tZhvs|!pGmc($c@+31tk10cHzqcIiF& z%s#i*Hj`9?JpxBCSM7ffp#J97%a_oi4L`NH=0E9l=B-WYHmFv*B5E4F(oy5EN-p?| z^AFT5f`jK+K;KXqU6Ttet_)>4@lC1ylv%=J&CFuw-8rgt*hqnNsLT}K5+&Ip>eVc)SC zMa>Nf30F_+)&v{EhyYhMzlg$4rDe)Zk#M(;BI`HJbYu}ziZ~{asmvEbz-irI@{%6j z(UaGoK6|=8JLWL8Wv{pOFL+4aE?E#f9Q{LZ`?Jr#wA7u_qu5xLnMS9mwa<+8wcfvd z_2T(Um$?8UIHhGB{Rhrz!-{$5TKj9YT&omLw@k4$plOl_4CzjXtMUMYWn|k1uK7i# zAUStersvA^A518EyMmI~(kDe%U#qbFZv+9-UMDv=y zE|!>W0xmrt7HIIri<7x4UR5vX8|xcr#Bk&)*HhCeYLu-Gp5UW7Wxh`>Bp_X}RvttTuil>hrHg#%ClcsBH`#U74wX0&WLH=}u*ljj& zR{2k;V&Mm7PR_%@<51kEh|jvY=jWcyBX7SJ0-o0y7tqTwyt%bJ+={gS$w%el;y4b9^8rYP_)W9LzM1aF{fF1CQTnmT&p`+I{wxb^NmXcD`q ze<;L&46bM}jtzA@xLbes-t8+FFI=0iZ|WXCH~DwQ#LsY!xEvv3Hrt-f$x)IB=qXhB zC4|Yb0$jTuCO4Dynd-sgT?*}++KvG5r0$o|1Vgwa$uT$3MBC=##Y>kPx`!Dji)`1c zZSNcAfMtKE2e_4<`-cke=fnu2t-x1h z>E~ZQ+XE+UZPvElNE^@A&5*230%078&leJum-?`f<{hdQhLE<`r*stiy7ls9C?d8u zIa@&XTzOx>$B^ZE?vFT&^bi8CD1o5W+3+BCy2i{uHU{mjF+vnHFb3d_>-+$KSbPMKDvg&cMlDVn_jlU9Q=YPHoVdGzE@2mE)DcQGM{nOf*;*Dn z!>M)D+0ekJAXn)1PJx)zepsfT2U;eHp|+4t>~77adDIKq(%pA~tLWzNh{?_WE83j7 zA8`b+Uuq5+MF#MPnLf}#r?Rzs02Kj&Fy(@OtSW+sxcvwL*e{5v=BlP9NQ-Eu-MoD1 z@{K!9j-!mPd0LQA9m?tXvamjH-;X%22Da~G=Oo@spwa^m0Fs%XGYiOmO1ZyfdV&Di z*O!$A{N$NA+?~ad?xtH;E`0LQM;~7T+1KAzvial3AAb1$_dfWY4=-H5e(8h1{r<;S z>K}Iwj?~8ftITa|sHeI9@`X#c!f?-ATm8KhY7fcrH>3li*^S|X-#CCJ?fl?1$=2mS zyqb)kr)R%*2ODN&3cwf~eAj$tk*nm_jqZq+#>zBA{07cLuzRj7?K<35H`@R#*M|tW zLg8Y@#r`1&op5gi!h#B!?;4Tu^$i8RPzC4#szKGca-S!)Svj}-Py7`tLM5{|-ba(p zs-;v!MoIZ*YS+tGy0)7?8f63XIW>DvMR%iZ-CM#eO) zc;^3Mx=elM9Ve3w(A95101~XIl_%>uX%#^I;r?7?8U9nLkFZxYM!H+?M&ceW+8i;u z2wX=ogs-oJY2*{AGy{}GIktu63a}@^a&VyYhem&-Js&b4HOU2)zXI_1-2QyDMZa}e z_dmUU_vx#XorQ_M$MrX^noYS@f2ZCMhO2z_^2PI?eEjhzA78k7=g#%>zw_ZI*XkQP z0+)(kp14W3dsi=AyIX%-S%3dwdGy>1Y!b_wo73=g;k{Ow4fzEYEQL$-^{dQpPm63L znp=@36QiuEc`AKhT0yFUH|etl4$yrE?11_Qw?8?(%dSPZvivtFOflDm zy&z1=O#7yL!)Zt}qxAmjKg=ZVqT^Y@CP5_3Tg|317v~5LxYo~I88k9 z-QZP2!3WX=*`(9~=&v2UNr8u|?c@o*=l2<;m)YevFfg;jCi?6CQkeJqJNF(p3G~QLZET>;Y{Q_bCN?}VRc4R5c&{Bds=s%D6uRZn9=o0ajVkB7xa738 zF*tOo0fjei5>>~QdG6+i+5mo)!5U=Gd9TUHB4WbVZ_cOlU$))>K(`PhQ6HK9oCQQ!rn8>}@j?MuCDx|1q$S%+_v6Z6aOg!>8L;Kc9{+Nqnb zGYrZ`5KBH)`hN)@7)f@E{VVrDQo=gAA2~sdV|c2YPuoxzm`z5coQYIbg>)h4f6hP9 zUhQz^w05$m;g1QGPG5{d$;aP6S<&5tf|P0uP_c+U-ut4qwlb1(zr`Eq0q3cR4JQr-4_w_n8x)B6Oo{JBQTe>MzF? zB2QU+ySh%Cy-M{m6p%)LXg|2E{I;SbtvW&c-v#N-JPp^e3JgiBI@nK;9|8$LeW$oeq=5TO`Zrq}SBA>O9t(x^2I^%5A(Z;~ z=p4f>vwz^8O8w=o3cWN1umWjdTGO0^cgcV|!E-ZRrR__Kw-c#b94$V*S>>w0a^#K}!Um60ju>fE zbT7;-*5=o9$U-=iSXn7cztO!&zcuQPsawQ}6yiW|`51qHasrq!ON8L8v6nl`|0A%4 zj0BZ&#eGda)&JB1HXyFnP`dbXA9L0YH=+UVb=ml9`LPP0wjP%WIQQe$qII81au&t` zXmG$2OtW-nn+6 zod2(%od5XZrAwDTxnKh0=0&3$NSg@U=g)um!N=#X-fjf7_V%>!T7$5cPED#d0~%xx z@N@{5~LayLfT2pjOS6xl`T% zhUG$5@Jr=-eT{le=3Um8BJ&{dmz@`>_cjg>b1-;4x}kB{y1h8R4sgG)hIC`%v7NO1 zA7KxCSJr1XZ7O5;|Bg|u2KZwbOez>bb^d;bDS%pAK>@k>=@ZGB4XWW>NeFc6!xewO%Lm`Q|IsIxuHLwk zMi{OPls#&HC8>3;J`)7v;!4drnTgK90pf~ z9Fj&Hj3z)$?BQN**Vk{^_sCf1IH8aTvQ9p2-VBv_IZ4+DHIqe|_ubz223>7thzbn;+i4 zeidOiL)mzHS8thgr)J_6FvJkjGCkY$i1tm32}bj)>UQM{dWNtd2his2NZ%?BU}|RF z0j7QSFS*%N%(HFmdISfNE=@xbwAc}xZ9cb(v|2ogA1{IdIwiGB zRDag$wuicC)n2`P;p6u|_~`tlYnN-6_HT479-KZ~>uP#Xe_2v^@#d`?*J|fq*=VWj znvkz)IW7cJF>h{sK)xbNci34O{byk@fIK@ep4F*sv~UHq9RKuL-sKJkI)}UXRp}Qf z-2%8~<)Z5^&gW2c8Tbab6pEA8)N;3!e2k_#uAkEZd-3MSzxVZHCFYuCpAFF^sQAuZ-df(|56w4Hc%D!e`%bL=pA1zI7dEaWWiBCDkIn~7}7 zenY}>p}Z{tGhL>XjuMgCm92x9uiw0W^H!VQ{KT?aDQblGefD-j0U)`Wx4-T*2GDefQqIn>XvVZaTVK?9PB@MEMAHs;|?oD^p$B zxYAkr*2yW345w)U%BnSe@#u~nj5r)|bLs4g>{Wg(38)x4f;Am8sd-u1%;O`}TGjHa zxH++8K#cs*Z_46}IDbh&0D1SH43-9k6@;9>_0qepe#RWQloSBz)|8F^=zD8`L=7TP zy$dTB)VIso%Lc0LV-@Nz7Ga-o3n9&Bd>tvrE6w_qtO-k~qAl;*Kg-0mvNH{JHcNrQ z>s_vC7yUuP7;})`2^EP~k{pGeN)M;NHWp1AZWlad-<(1eK`dx?FwQcuD2J2D3mTys zB9P6_o}2kPwIaPT&l)W#O|q&g=YUHb0sbnI283fl^v$c203goEsEI1%H|FTA+7W&%CYuEAZACjQX${|^!6{JU$ zr8mw7|NpA%(e?8mz5o74ADhs+bk&sJ^_$o0TSqK?K}o-ROZ2zcf@NI07L~GA!WVdP zQeJ5XRc%Af%?)=O9yOTX>_C^2|H+F>|MOl2q5SWG86Utt)MeB#+d}+$VsCBp7s6FB>`_2s~La}TBt$Usp4Nrvw1&8q*x+_#lU zRUH`re`NYeM(D^=6()<%egsa^=`n+Cmw83L zX6xY1j>3uZn(yE+F~a#;ph1*IJX$y3SCZ6265ib`t3UHs@-~zPX^l)))0i8fE}hZA zjP*(hB$p}Lh5bbT4EmOJysmVa%G_U7K44|eR1BI^27sJx3QUwCmvB^Tgb6_XeG4Ig zXCfED>yVwuP_iMG{{HsI^|x-_zE$7cJ1P)V`PcF$O<;N8+2H@X@BFR5_1zB@_IIo; zx=D;qw(kqB2nbiNIsGd}c4$>A`Oj8CAP1sN9%V6^Xougdqvsq+?K=w+$_A~eqc9dlryvssB7@f75 z$*~G$oB>7ubtr?%CnUD?@GvVe7yEIMb`Rft|MNF4%$|vMy$G5`d_E67zqEB|C$uY? zSDibnyDB{7?{b~R1NUAG3&o?%{6yI%sQ9Hnr>v=J+Ni`-Iihx?w9h9B5)yb%!WWj6 z!X=14i(}&IXc^kk1d{#nrfLT0!pu||`w1_Q#@z6*t@SAn^Z>YGlmp2l!K(^c=<7%# z4u|Cu`g+mjhRix3u^k>(lmAaY_|A72fO~xJz77LIDXx}nC%3e4%AKV|1>?D;^?yx` zr<@~o!UMSMgG2grVwy}a`|!FfWK{b@`$J<;AxKpQLg$8Vc5VWT&0O$2E|gYmrGcVj^8OcANB z0xR+z6WHdcWtdd#LvJlWI#!8J1Px>R^3uT_DZQIHkHHZ+E8UIbqxZJo+pIgi@WFR~ z=cCImx(WQNm#@+^L)ySFyK&>zt#W+{uFp8Qfd9pWuHpEOmUOf#S2Ug?1%Iv2IEQ+_ zp@F7Fhua>-!=cbOGu^EH%Vh+zOzVI*jRB-(3bK)7fo`ZZ?;b?Fw`}~#$@8pMgI6Oq z@;9#9#GO#R?g{d{`guO)NV6A0hnqkg5#oH8sFL&HapU|xR6xtC9Q=h(1n9&B*j)U; z){2)sPFv7z<+5V}5>L27nDR*~cMMbYYF%}Xb2xi3fzO^lF=8F!t%Y$~NhP6w?}>$) z&b6%n(hIX+Wm~$)g~`Ax;DTSMF0&53d#)-FBeV~zj#YrqRD(JiOe&TjB`EWzC-c@5 z_w1LK2*W7BRMY7Qjnqbem4go|5!Xi-IfY^O+uX& zH;9U|5m7EWlvp&?(o%nLG*tsHz`KTA4(;I|P@a%m0Ra=s`tf8rS%m>g72DvIh1{Gz8fj+_vhA>+P7Rxz(1Pz9K^0jg z{kKu~$M3)Y!Nt4HU7gOm{(jTs;c`dz7Is!#uUmKSJv4!77_tw6d30)Ab0?FLLJ_{& zX#b!o4Wsf$Y%+=G2>CjM#4I)%)U?+5_&M~N@m2+khf0vqj7FTH_ZSjwjw5=95jw-M zw6bDL%I2EZbn_P614QkeALjJ@P40#CI#923ht(NLbl1&wCBfq&{M{i9f^xW&JkVAC z_Q4CEnm=#{Dr7BS0E}lWk%&Cxl3CiXw-pstR5`ZjH}`LaC*N3&l(bfgdkc12mVYUe#38AMKn)?_t`DgY&tPHZ zj37$z67Yagq6!S{KvT+(sa9Guh>?iyEN_VW+jZZ(|NeJBxQ?YS(Z6-)!O-SRBSK+I zTWjM3tL?7cxJ`}Pm@Xi%ZEzyPcR0urjOyG6oO=?zo)FdBdtCi~Kh4i0f#wINOc}uG z8jvU^Nez$>!L^^ON|&nVeMiU*_z*u4BoCD5e$L?Yr*u#Bl{3?a zLY6LjFZPoZZwk&9!6EZ+tRcCcB1QXQ=>*l5f1%{px`+8}eDS+q*`p{jrG!KRb6DJgj1a&lp_`LmuFwz%;u+v84jRF61s(0!U3b;jwUj*-iA3C=Ah=zl(Bkdb z{Dt5@FDU#b6J_z3WURT15DG!&}eNlSDWS zJ5S2bih==ie7r-h_sY4|6G`Km-u5aNCurrpsCfdMPL_#(8l*?G9asEut{!#uGIgB9 z1j4kHATeNgVz9IA(E}>exK`+PkzTGSS4Jmf1!*i!&1yf)e`}%c@WRF058<{Ax9{9< z-+uGv#T>fE!}@aL7xtJ7{@He%`QzvsD=M-qrQgh!RT##UhU~wy(!{rPxYJ76s$p?% zY*Ay>&(O+^)DrD7Vd{r5TxGHcNEAmhJPixTR;I|f=a4*K@skrsX129~u$J1+3e<>7 zr7qy{tDMfUo;y6X782G|4X)FBaPpC$v!+Sj_XO#Imc^RL5GRm9wc62hWF8p-=@Sat z%D5$K2vT#%2X|@&i&L5!9-$M*j?3JS;`pmx@xE>|Y=L}Uj(^{Fx#Zxv<4oFH%`XT}ZT^Kwy?_@8 z{|U6`!yJC4CXn^-OY*x!H$Tkirtij~Sb3ykQPl{IW#+F~lr+xin zf}m$^X|ns#{VV4`c>jCvpTBa8Hp|VMckW|wWhGfhdyBdu;}?Pl98w?8W>rAc$Y7^_ zPfus}h%jW1feP++{?-fivVhx@GgGzk4ZTp!unN^)UN1{;xjoKLt}6kO3Gl3v9JFn> z!&O+fd^4idp9-n&|F%0q;wBJrRlqCo@+)~?{$D#@(#i$}9+Vw-mFG|c5JWGB|6cG6 zT_XTcsgep*guX?kFYH}c>^_>}~j$k1fXe6iPQSAy;w)&C*%c_nAePxfKZp6>?7 zYs#@*P6SnS^_OYnjf-z-z^exW?HCr3#v^+qL!adm+*H5i^0fvk4-!g9nnOS&w^0UK zLQSx1QMj|iAm^7XJ$rVb2G~}`^qODfBZ>AdLNXA z*~X^Q`V%5}Qol~SY{FK6Is~TlHs*;Uv%-GrghtyY6fjk%$%o3+KPMF8F$rn3>!Yw0 zTr<w&t~`t55pY!a zfprdR=3AUUihIOoUHhsY+}`omyv6={RT%h$=!>h++*kSI`uxzaX{-kygQ~@`6%WCkI&4bB%b8n(bmSdA&CN6)&PKDi1Tp$2^wh@ ztkjlH?QN0nCJBgbdjQ@uXcpPgwO{{H8c|pZi3w zaHk?mhdb$+Y7tJ1)uvv}NM*ivFdR6&Wx-l2kSb1;MQn?9!H~d<{^F4T5ti6Ow$4dnpq?3B(tN;N-=Dun6M>h5$QAmX(%Z zr7zF;g=4kx6`#(u`7Cw*O5Gn2^B=+A-$iB(w0NmJNX zdEG<1n08*L_ICJbsBoq(*RoCNj?AMqYw4Qm>HK+WXo-H3`uL(4V0?3RSzTq>J3quT ztXQ4m8wI+D20`F=#EgtB#z3FyWt9bI>1IL~HvBj||l0?-Ra?b${~4m)To z`MW7`7atHbJ71j7*L~Ag z>8g>^s??6VB1hHu@UEFWW)Zaaj%twZRHvnQu6zblB19g%DJ|@DtOT{;wl*O_gTbqT zFs7=Jg;oL5?N?amU4nJM^(l>Q=40+I-QNFch*rApHbQYY_!A-%Qa!F+riGziZE@lK z<-hWM*_~D#6UfIc`-u0JZAsX2Q^m2s%hw`S`8qLHxT1OC0+fFZuQ27aT(Jf!HkfE!md`!stSTTK~A9h1$IE*MXe z2{&=d2|duoq1M*kp@=Ry{mI(Ie;{*^-&HLMg;I)~fNQy548iz(&P>b4)yC6JD!!*& zx83dJsZ38TpO7wp2j!%99@kx|bXDA*)^Vu!#sRdqbl7s=9;JObq_dx3Mb`Vx*Zu2=ayk5t*2kK-or*+zPwrg1diAGW_V?y-D?v6>Bz#r2sHGMYfHH~XALV(qSkv10wr%{m0RBduq zy~lD_H<#_0{#-b74ZO!g>a5|NrtRmUi29k8HC3M``zCL0IgR_U4z_pyakRc48WUfE zlB>`XB2;_FmPJ(I-sDBLA65yShPO`4Q6+&5g{dea&c|K@%Tmz#>>h;2bisX}p_xL-s=8sV)90 z-2kA0qX$}59X!OLcnszhQ;-NwGbuehcE@?77>QuW&KBu0H_VnJs8A1 z3v>?SQI%5?{=MP8k?JW+01$^<#i{W!xgn>A0!j}8%$yt8GbD%Fd(P#m^){2JdOGbFALH-sQ{YbnicqG-j5gr4^y4w-2y9 zRQg{#`|(K>=1>utJrqe&D7{UCG2|vu7D&$@w4bo4PvA^bJdq zMsYK4x;hIuoYQZUC9w?7l-CQfH{H0@ zGiX|eV*HUvUykPU(X#&;0uH|l#gEs^P7}ixCh3IRD(<`6j(+z5loR+ydU$ktDhCkL z{q`^!qmZA7p0jrpN_jpOJB#bk6w1y^ve!Y;GP0TC+o7vB*Bw*UU`H+VDw$Fc1F+k& zSy|R&{$z1RcV>pGeir~$C?MJsuKA9r6|W*B!^)u zu<^8ZMFFs}R~Rnq#^$-CEv#L43kbDlD;fko zNPTufK9!;JLPlA{$JDt7D?o3ffaaI0T~}P8fezjY^Ke?TT)S3e_cp#eotc_O`rrbaG5^SO`|PyV}VI z6m*U|%+xBA5IsyJ_*T0~`O~i!TEKvcUYzhBg&mX*r$4zHF*xcVl6q>xIwEcizaS@I z!Bq!%sVw}G`$-*=^4}Gg0@zQ%--azJ@M`j8g>qG>s1r#a!Y$#$bTd+vz=`OLx3#yI zw#BaQthat|fbS6Xo z+&v5c!_+cnbPn_BZ5jglx?WiI@>1>3mbpIzb$HfHIGL{)EZOs3Jq*+>Rg^Q(@6B73 z>aLpXtqxboCHWNqb`L|~Vgu!92Y1H`1C(CRC2$@f=Uqy~6J$|kz2g-Hken+Mh)PM7 zI;It<6BxKxCWrT-fy}d4b$>Rdm9;l&&2$;mB>Cz{frb+}ZW%KE8Twz9%{n3E4|SeBoqO`U+i z8d9a^1k6i{048wyzTLe@PeYBa?whFFaO@7A>nAG7GFKkz2k<2Qas1K|p$LO+>F=q2l25CUb7@^m z*;uA*LNpOdLyJ;u>3ycN=h5=0RhZb7##hJx`V6iI;qDxy4S=x&-V+sIe@|US!)rCB z^52i2LY3Zpb?8h)tM^}I_@jba2}*6 zQh{|4fs~}--|W!Ivn`OqK>UUt<1wSDzHLLRqbd7GdhhP7D@HjT|COs( zO!UYJdabCkdkA4Ocf5rQXnOp(wZ~kdNr2YJtt236ceSLuq`j~8z~uN&i8T!cOUn|K z0$F;7nQxr4{ClJm2HAxzu$jh*_ouAUF>GZ zVY+k@MgZ=sgDkVb_tN^W#@F?D0t}jK&w6W3f!guk)8mVikY=Xvly~_#9#b6Y?CR#h zlfCEF!W$cEQjZGKxiX>SNV+(w+UlIID9DG7b~*E>t9w4hyUR>s+;!MP>$TLHmOn^6 z>=;X&3G$oj5P$JEsr$2<589hOE!<+V#aN*O1}ADG66w%v6foqz@iIZL9KO0Q7;?g8 z}B-_bk&zwL4YFvaRMTQ5;%m%P%3em>m$z%il3%yc-p7;8A|9`FN zKcG#Ije)tz2JYXxdHKR+gB|uUG!PuhysIAYPd0k#2TT`hZnPzZky@AhoOQz2QgW z0MvlF;A!V*+vhM7(m}7>f3e<|_ZJ=Ft=)0b^L{;fTAu9vx6^m_r_cOjL0~!gaTN-D zr+l+?5V)VSBwS$PB&`}D21cVNcOPZw(B7NKxuv=nkDTsVVeoC!q$*3`Q7*0=-jT2d z`@>j_yL|>>Zb1hEXQpn$`;9+~*yy33>w`KM4pWw3v^Y+ttY=!a0z{mtBQ>*4=0uE0 z4xHYnmOvGZ50DxYfINC*p^?yXHwELG-_?F=6UZ!vXmkGTV)o60mrAMjf*_L zjTY~X@cl=Q-iWDl>#xQ#AKkrk^}Kz2<2@4T$sD zWI6o)4D$vI!2BG538sZNtHBfU$(T_Yc0Q|M{@TknR@-9C)wqy|QU!Kf4{o0c~e!%Jb z$HYhF5JCg}Iq&cAIgV8J3Ariew0IV>C?f(*T=>3S(^&+NQMnMbd1#s1o@`DsRzY(~ zUPLaAJp+O+auRz{!3YK+I;$Y31tNuDD6y=0S!?B z7;_ib)osgV{^$COwsHl*Oej=9Bwyktd4QBNK^!v*?Uv>Eqcfk{RW9R?TuRE1viy8S zF2R2-EegjK!v``uq+f4BQ}_S9q4&n>7DuNHDz}#A<;I7`IPcuX=>6=b)<0RkN6la& zL@pAF{R7A$p|IF7f#Jm-`Um>U;IC;u4R^FyU}bkrbGsR!5czH8tr%&QV0-jQI)>%H zdD^U2S}-=wKUIpw9`vr%-ZPEfVs?<`+Ev+j0%K-0gV(0DPeE2MM*xt(ox}OF)+0|d z7f~8Gcbu`z+2~a*Y5N zf4(Kb*Z$+UorV;eHzqk}f^L&R%H5pU&Z%kX6DLwpd^icW&*Kh}Z)vHvtP8qSf^?)v zba;}Uvbh?}$_4fW%EGowP9!X40368ImE|28>KQ@dEHnpk1mT$8r1vK@s!jNbmHC)J zZ6(Wm{e%6x`judyU!nZdn2ZKSVyxoSPw@c_S>%qOvk4DbRj5l4UN9`HZf|OyJFhge z+pL}k_1wK%yZ7pT_D*O4=mCVpFB4%nDhR(((1{*p=p|ZDauiaX*gu^Sv5S<=Z_gcb zw#j`zshr4C!ajU$%|&8wI_De@c!lGHM)Othmih{ zX_iBXax`uncps^PcPU6f00*QYyXjK%RI5sfG<+X zjB~EnElxV}?(Wk0lMc*{#s~Lq+q9HGkIi?#(Ms}eDZR1)IY%c?aW^tSRX~K09+aW~ zLjgJ7WyYtoGXSx)FpLj(oex#ZpH-XoSv&LO)QLqr6M$%*#tGP8CabaTK`XC9z>$lG z%`2dt74zh+t~H0tanbm)x0 zp=Zxz{Jw)HM7{q%`+h!OlI}-o)-tXP%<$I1j8hA6C<;d-T@?kv^_4cILE*HRU2u2V z#*25KzI_fYIwbj(R*ea)sxIIB8ymI<=Wr-SY}U3Vp9q_oj|BDAONh70=(|q zqI0bMW6FEgJ>Mdp??aN=oP1&*iVx^J$E`}jME`9GDOu1A5IqGe7d>c_F|#all#52D zN>E4{RsEMUlY7Z%A%97{@k-xY*4%y3*BODehXj~zUAuby`nBt~a_EnmO<(BO50-(g z%F|bv4*uY%ZoWFN0W&*UoFrSoMgoV>= z#Tj~FxYB5g^tF^f6{SPrX0@0M&ST>BJ*`yqnbeiY(W5(BNQtgq`?WMI1KSmBlf;Ud zcsOwlJ}w^&)kA@wDGv_bG1`6c)ac)HB37Y;=q4$xG-Cok)L>{idpX%bfFg;chrOQs znxRriNX5zzKTq=^qlYq&{TH8o`tJ49y$wXLv&BRCmyM;dtYGjm%V59e!VV{*XQ*s=dYi`&&k!3g(nI3sNfq3|LNtI!#NJWcD!#E zKdwQ>pCitv(regX1Q>ft;6AR{vUP?Z$!gB!HNR^ng85$^@LA(%#0PsOWyCJr1$u8o+1Wk+c#F~UGBE_U3$f8a<8G6v$DI8ENPwsPck61}Sy)*ik7 z{uf`pdvUb$WKaFI7`R|nw4YU4t?tDH7>(4CBNoS*dD;AB1pzw2#NyQqaRYi12Pn_8 zoLqKJ&Ko%*Jrr9pJ{*?QzP05+q|S0#?ac+ZJwqc1=v>iGZTmQzYf|baKjtUgc=qU% z%C>R2vUZ%;!`1pPb4M`=4>6W(b?-*qrcGM+tj;#GVbSXKa{HU8?u+0gHNy@ZFa$Kv zZ%DV3OuAH0iilT|-o8KNucA&2SuinF(L7KOczRt(?c*r$jm^70a%53o60k}YwJMc7 z6<3a1)H%lNv@^o&!=$khj z?3BO$t?erqv=I;IohJ)9i5Z<}B)U`P>=vh~!vK(HeA_xlAd8e z)2ck>J>qLB& z`Mg+34jpkZWjc?$wNn{Hm90x}pp=;}O}yJbuw65y2!qX2am}^37QhX`2aaK2=?%w6 zS>bWaF{)U+<8rXI`(^S-rT_fKpSBD-Xzracf? zd**W!?2$g)cuszJY%naKwr1iBT8SAD)G5N$N`t_qN9>ubHZM-nTn>;hvsmVTT-|ul zAl{y*(co7UGrO(5DzGN4NypSR$iYim%ML|U@4Gs9(5+L$y?L@4F^=%@ja(lsW_9or z`YdbFAGI|u0)*C$9+}lI2dmzGkUxt^YDaUtL3#Qb(8r0V^m}^x@|9hbCr=MDcuNS_ z#mmnn?Io@AdLs411QI!w0o4x59atWX6WNwiSF-(UCCPz+fr28m;0pUFFTm4K_jKk; zhNRu#^~4B{RSFr%As`TUR(X-?#(T>y@g@n83ipu?l%40s(XkttR#Ltwt*U;co|2v% zdTo2bAtxJtnMBF66AbVfun5xJU^MZ4zGUCQr{+snGR(!St;aTqXW?ixOf`zqK8R%B z0WGHM*K}`;<=!%u?cBBXOUI`sa!h0y%Fz}Ez8c_cYwH2vp0UNpg#D4hp3?oZo=|tv z*azBI)P*3<=^|-N@Om@cyL>k3X88Q$(-d#OG`zCztcsd`*dpD?Dhlf7rKGDUx_H|2 zYJFnzRtS}3`cFJ=p@4&4|acYABPqsG&?>Vb5OG&@A2b_M@!izwI=t3HyHf@$)OlV7fy*Ed|5B) z)u*3-{n^`{Wk1Hnf2H@Ij4Ie_70I=I+)=ikc$Yh*jYF)oDqdfbtKi8;paLkHcGInl7V`&A8^++Rh{!=6n;uhy1P z5!0hyUn7e_YZ_glWK8?(hm;oY+_62k{$90LC;hlHWPh;FG^XJzW0!jUeL#Rgs*vF) zj!#hvB`80*Ydm+%-2c3UHX<6p+OqUA)ueh7ylAU&)wk9!k^7Ap9m0_!r3dN|oi)(Z zryh(yl=YMU-?_V{%5_zGP`MLW{PI(mR(=MoUlaU2@sw`>+9hf&0r|@2l3Z9o6+58@ zIeHJMY|nGIvg3Joo9D>XwNE3n6E+|#aD^ODVIVv$C&4RkA=Q*>OmxL$#j3{L;R&gR z)uF@06S{fnAyC2Qy+Aq33`bGO{nqV16AFva`UIRZr`HCV?Zxbj9H@B z-|b3UAT%&2eMkK4=;$M_K7lYmGoMn}_(*MVg&G5?b7@UkI8NPCfek4nL5KNI?4hiN z2c67seA-b5?|!+hp5YI7@l%%=rDx}UGRWNe-0<@BZI!M+dzQ|eU*yx$l?TT0z-6>3A z31!vY-`V>c`?g0eMa^Q%12qd$%>Qj*4ckQQS$lyH!qwB4pMLSf?|*TW;`^;bb+H4u zFad=)JJ^jX9PlYt3AzmO_lK@uOpg9r)e_?L1LR;NrfyB1kGUkAJgMr96#S4yK03!8 z^4n8)I5~Y9Um_7-WG=Zn{2c!Z(IbWy*4c8RLh@c2CgfT%vbmZrrHjUP%3j?s>Tl9s zydk+K!$2xpUl+-v1DnTKzSg{e&gE0wv9-LLw1Z6nw%Jo$Lk#=t;eMz9@Ky#OpAC`` znaH(pd2{yZCyIe-Fjbs`#GiC6_;oK@Dq2w%*r0V%5-x8Y-sTwP&mm-#<6XFWB04DE z;a4YIp8PaBrB3-;M&dFiA&Q`GI63LFRNfX-BxPGw)1>QJs38DR)Ik!1x3U6+4G-^d z2i(jpLA#AmWugc;9%N=tn!W>X;VH9r#qlv0T?4fhN^#l!flJAOBsU-w`Q=xizT29I z1wVT$nScjwF)+7NtcSRo2rb(yzCoN#$V9V;(l3kbQpE`cK2h-zl5#o}wcSHMLio-% z_rHXKBqMs+|5PxCGduRw2F{&BaFRA~pFBT0;ua5HSeKm@>`7SmEG2+FNTESItN2kH zJguj`&Hcp9Zttk=59)sLFnON*9yhm9I}R0G)fPIZ%HCq+5Q6bQf%yYu85MlPx$goxR@{GwX26gM`%AU{Qc>cE*vB9do??FzV>3Z`hRl#Q@VVID@;a`!={VMCbziCq*5%B;{~_Q z`2S7_a}ii(EK))kQ`tS;f3lBL$~!BV6d?v<7Oh=PcYSf8HvfvzIv@^(0GwwE58^I& z@QEZ=vW`dqJUcu-efjpQ?|=T~-ZHhQSD!vR)CRB7aMpnYQ|);bf)EnO2ggv_AF&zq|R{j`t@@4*|l4D?l;j| z86HZ{whLC1*?R4R`k^X<%}M-9!>vIpV7G%(XwH?aA&@WW6@ zHpLw}UAL3>_lGzm6y6l?sh8y9&OhA@?@)Oo#&hr9DZih4F1nU17C5y-=;djm?emuw zOL?FCzcs_#VuD5;Uzw^+O(l=wSYck?dprP?wru<)uxB2)EHMb)kr$A<;^aWrLk^Xk zl;}TeHGeI0_gc#eVu|--V*Cl`9Lq^RAo;YGP~064RZ&t9jQ_4M$}0|=FtEU}s2^1l)$AUvC{i8fR$fZ@dg zcHcX!`=+D4`4L5~`ykyW<2&{+Wj%1S`Rslp`Jv>)@i91z-e>3zmOC>$j-8Y7#7yQ5 z^!E)@m{i$%Y;amqM8SOQz5m*bo7NUKYdnE}^@s*7r zFTq-g@sySn13dHcR1^zlX-7slb#kcm{&*lGt@ICgJf6^I`elsTu)9L0bL*-UEWzBB z_AEDHdY?+YU9r{Ki8-`OY+4z76vj*^#prnV*c{E6ovg<`Bpw`ah-2{zME}ub$ew z_tFTXc7Pk1(lXg%sytU8q>(R|`W@E&BWlXHcP-7(d=>r(j^CgqTAjAWm<}GF2G&)A zu?l===}*A@g=q_}G8P^+Y5n|B0y`+TW$mk6bT%OMn~faTbeODSm2*+dS7f|Q+#KUr zsAN1vmd~Ry>*TIlwXl}_FXzt{Xv96OoxaNX+cMzLU%h<&_Vuf57PM6Qh0ITF&o$Q$ zU&b}N_es3bvHh!%&NEIlP{i53^O-t{FMoFIts`ixWH*wL;~?=y4Mqm~d;3w9hV97x zO(l5)!&psHCIEBLx&Dr;>K^%t?xMC|;)A?S7+;&AnFi2j=splS3g1#)&cml^052bz zEQKovj8?}1rrdvef`EHR9gS(%`lyCK5}!_YAk8UJhHFGb^{3)S%X+0j!0Nb6-71^( zgvRppXSpA*o*h@GU#-o3`S#_D*A^$IO00^-7*t+!Y&c%`?DUW7=144;`#-x|f9rPr z19P2Nei7R0Xm9H@8aOtk_a7NX8*_1Ssv_P{#uQZmr(dT3%HC{_A*Ww;oy;dB0%u`R zW+JVOOv5qtY1AzA>_}^J+^72|9;_GRZR`v57#D$Qyen7@>cy(OPI51pA!W?L^UAX) zRfE=lUcY?#`W2X7zt$I?oceUr8>X_oG1t)Pvp*K-^00LVARajXviy!43dFj9e5x1j z&`>B9&tZImG#o>lQh|G0nqV@!4L#C`~l4!zw8q;ZY zFMaPol5Tly04VJZ-b@UG&|FM``+(+-gBPwoadag z`u+6vrYzs~QJ_upu;@SA>lcPM7f8Obrf#%%v?JN#jqq769>@<@W|Sv_!}I_fgUlm@ z6NyUpZtgVpEI7g$7t|C??T3zC*0Pim_3s0+HPWEO3ei(Gs7}i)V|5;8eBnp+-9-&^^ninT$crBMcY7?un;{XVn zv*XcZCg^X5Dl_y-0wt8p2TRC#c8O#@4{(E_l&L7q(kod4eEE0}D#Xn1Q-1H!fN~T* zzZ1`hkXD{N+VL!OVh))!;WWGLjgb(5-Ru(;C3lv8cYwfwUy>SL@Zv&bb@m-Gz=(=c z%XWk(gh)1LK81Q@KKPJ`7kEb71pE(hg1qFeSi3jvZ$DGL?5LRtYw_w?$A{OrR=_q~ zL0>n(BhuTYG#buFKZc=;9DOy@oJH|elgAUr{9;7-a#Qp$Dv~68ZblSRhv<8&+8?At z!@aWVG<)+T(J6O`_wmAp%(M)dFt7n128PH%IKFD_yXieZt*qLoYCB(+v`8MU$6d&n zX*tc|#ntK6lZ&gfsPit*89{YHnNcEL0FdVweVqFgz?U*iQ7y#plfRPH7%1_htgeAU zyxkR2O(ie3k@po-ZKfz7T+la{7JFHZ#rK)g(ZISd9qGhnq&dsrry`nz5$mx;%_bLS%cC8e6i87m5_y`&_>EeOawUy`yY^L^CXZbSK? zcCaSwC-%7#?)~cio;pUk!PRyvxdt|~%{eHd8{h>_&p&qB@j<5Kndk!AsMfK6g4~FCjeE;vt)kpm@DZOR6A~9oKW>9U#o-8J z&Q1``&0nfoKfXqLl6&fyOgzhQIYo(U!F!yg7LEC~kv>Je28ZPZqA(B?zAVeO9c61( zS%CP#(tl5?%DIl96|MXj}$YCjJSZSC^I0S>ZF-CR|%5wBH|%1?}~~< zer&b;gEY9opFV(NUUp%nZsC~W!A$O0h0hB2;zvjFMr3S6B+huNv7`{S^3x@y=URsXO%4KxWEDPbI)iC)P9JAT5TKAc!{+YQnfk!j9A)sEH zDL-*GvS*LbJ>{Zv5fVE=L$y_pFoAIw8;o7?$*-PVK7TH8;q0wQdG_q-bMs?A4idO8VZUm8A1GyNJ16P#GY0?!vylu5p}>&y#Dq||KnHF; zfR8`lY%8WNH zQ#)?S>bbwOwW3NTQDOtt+43ddFZ*M(#3}^Fk|ockq=SM+DUDoy?4o1cyj~ieQE}b& z$mzALgW~Lo?}w_9V-6q*ykplgG-u3vX8%HZ)#A#6!B7hMGmUEgzoi8O@Joyj$XCxX zXx`$<-2MQ9RoirBRv6w=Gc`sBgRTrvByp?FIU^lCZCy@4^K`#;R10QNWSFdkNn*k0 z^8?P#)Pe$R%Y#VD*T(%N5MF*ITCHdw#N%U%HxxGCyVP7Wa5WoTg>R)~4MwB2;f<^V zIYyI!}S`5b?Ih*h)xZ>>J!B8)Tke7y5ENSy|C`1OtMWDb~f; z`;V7No4T94TOEEFPmX4gHH2N-6Tu5(EtNz%it><&A56ls--@VdtLCS8L9Z1LBP~2> z1BzVBjT7ANI`t(`YoNxYDNl0^UcWYVT1fmUg&q72l_yFk^F~ZzX;n)v9I4)m%mvX0 zt~ZU_%P8AI?iMja$|w>)3L}MWf3~qg<}Tk(-k&i_m|UU2EjR3jr&ySn*57RW1wqJa zeZ&HsjOo@d!C=dTOb|I}2Mi#!BD zl*ixMiIwFIr|D5y0%+L}XbDiyVP56JB!J2I3^{xSX{^ z_fGy)?S5H-mLZVct~BPGYaXovbtDr9N4pkuZG%?fXGnwge~60RIemP|ZO1pkTqa5E zWUm4UATi+@oPXrPti!>3{Wuuu{$V@VKF;iqoqEnc;X0l$c_72!Dw5#Ep}X8A=KDi9 z5f#R4*Uf@I7*zul{69p5(|SInQ#SfdvMbDw+SqL{60$mSf4cjoG( zK#u?kHuhZFW{a&D0v#3j!cx`vp&qg5gHyl(o3buZ>ln5}SfE-~Xp>yI8-Ra2Y{bqU zy259*`h?hNxiK{xmQAM#f_P!XNY|zeTIZyqi-{!9no2KP8FLR$E|Gh1f4Kg~PoF0I zKbHDEuA_qym|{yg1ifbB-Oy;XY!n1)RJEhw2o0q8$za8mFI#8W=Nfn9- z6!4ZG6oHGn0t6s6%y#9&nr*gSqnG2Q)h9kaEU!)Tb90FZT&zG@dXpTxN0~7!MK0>sOu