diff --git a/modules/build.sh b/modules/build.sh
index 06f70f778f761f71e4589f3f56751ccfaeaecfb1..084991b75a6c9b3c53aed9f401b0f80563895ee9 100755
--- a/modules/build.sh
+++ b/modules/build.sh
@@ -37,6 +37,13 @@ cd cmake-build-debug
cmake ..
make
+# build VeloxLib
+cd ../../VeloxLib/
+mkdir cmake-build-debug
+cd cmake-build-debug
+cmake ..
+make
+
# build ROS stuff
cd ../../../
catkin_make
diff --git a/modules/catkin_ws/src/CMakeLists.txt b/modules/catkin_ws/src/CMakeLists.txt
index 3421623bd042f7eb4d2675c8c9ca624feb01cd4f..581e61db89fce59006b1ceb2d208d9f3e5fbcb5e 120000
--- a/modules/catkin_ws/src/CMakeLists.txt
+++ b/modules/catkin_ws/src/CMakeLists.txt
@@ -1 +1 @@
-/opt/ros/lunar/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt b/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt
index 44cc9d4bf55e9a02d72b672eaa62efa589c4cc96..0c7cfd571592fffd5413c021a27271d854bcbd0b 100644
--- a/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt
@@ -42,6 +42,7 @@ set(SOURCE_FILES
include/NetworkingLib/Error.h
include/NetworkingLib/Frame.h
include/NetworkingLib/Busyable.h
+ include/NetworkingLib/TimedValue.h
src/Timer.cpp
src/Time.cpp
src/Networking.cpp
@@ -68,6 +69,7 @@ set(PUBLIC_HEADER_FILES
include/NetworkingLib/Resolver.h
include/NetworkingLib/Busyable.h
include/NetworkingLib/Frame.h
+ include/NetworkingLib/TimedValue.h
${CMAKE_CURRENT_BINARY_DIR}/NetworkingLibConfig.h)
foreach(HEADER ${PUBLIC_HEADER_FILES})
diff --git a/modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/TimedValue.h b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
similarity index 85%
rename from modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/TimedValue.h
rename to modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
index 3650c19d37773788fdf9eb1d4667fb3130376e3d..10cbe19b50a0c94b744cd435b17cd7507436c95b 100644
--- a/modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/TimedValue.h
+++ b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
@@ -5,9 +5,12 @@
#ifndef PC2CAR_TIMEDVALUE_H
#define PC2CAR_TIMEDVALUE_H
-#include <NetworkingLib/Time.h>
+#include "Time.h"
+#include <mutex>
-namespace pc2car
+namespace networking
+{
+namespace time
{
template<typename T>
@@ -65,20 +68,20 @@ public:
T get()
{
- std::lock_guard<std::mutex> lock{mutex};
+ std::lock_guard <std::mutex> lock{mutex};
return val;
}
void set(const T & val)
{
- std::lock_guard<std::mutex> lock{mutex};
+ std::lock_guard <std::mutex> lock{mutex};
this->val = val;
timestamp = networking::time::now();
}
Timestamp getTimestamp()
{
- std::lock_guard<std::mutex> lock{mutex};
+ std::lock_guard <std::mutex> lock{mutex};
return timestamp;
}
@@ -90,7 +93,7 @@ public:
TimedValue<T> getNonAtomicCopy()
{
- std::lock_guard<std::mutex> lock{mutex};
+ std::lock_guard <std::mutex> lock{mutex};
return TimedValue<T>{val, timestamp};
}
@@ -102,4 +105,6 @@ private:
}
+}
+
#endif //PC2CAR_TIMEDVALUE_H
diff --git a/modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/CommandReceiver.h b/modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/CommandReceiver.h
index fa56c7a5e07c0ae7e00d162f4df6f1f38f153b1f..67edb7922a9ce569be1df01fb330ad1495b5e562 100644
--- a/modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/CommandReceiver.h
+++ b/modules/catkin_ws/src/PC2CarLib/include/PC2CarLib/CommandReceiver.h
@@ -7,8 +7,8 @@
#include "PC2Car.h"
#include "NetworkingLib/DatagramReceiver.h"
+#include "NetworkingLib/TimedValue.h"
#include "PlatoonProtocolLib/Protocol.h"
-#include "TimedValue.h"
#include "json.hpp"
namespace pc2car
@@ -48,34 +48,34 @@ public:
void stop();
- TimedValue<bool> isLoggingEnabled()
+ networking::time::TimedValue<bool> isLoggingEnabled()
{ return loggingEnabled.getNonAtomicCopy(); }
- TimedValue<bool> isPlatoonEnabled()
+ networking::time::TimedValue<bool> isPlatoonEnabled()
{ return platoonEnabled.getNonAtomicCopy(); }
- TimedValue<bool> isRcModeEnabled()
+ networking::time::TimedValue<bool> isRcModeEnabled()
{ return rcModeEnabled.getNonAtomicCopy(); }
- TimedValue<platoonProtocol::PlatoonSpeed> getPlatoonSpeed()
+ networking::time::TimedValue<platoonProtocol::PlatoonSpeed> getPlatoonSpeed()
{ return platoonSpeed.getNonAtomicCopy(); }
- TimedValue<platoonProtocol::InnerPlatoonDistance> getInnerPlatoonDistance()
+ networking::time::TimedValue<platoonProtocol::InnerPlatoonDistance> getInnerPlatoonDistance()
{ return innerPlatoonDistance.getNonAtomicCopy(); }
- TimedValue<float> getSpeed()
+ networking::time::TimedValue<float> getSpeed()
{ return speed.getNonAtomicCopy(); }
private:
DatagramReceiver::Ptr receiver;
OnCommandReceivedCallback handler;
- TimedAtomicValue<bool> loggingEnabled{false};
- TimedAtomicValue<bool> platoonEnabled{false};
- TimedAtomicValue<bool> rcModeEnabled{false};
- TimedAtomicValue<platoonProtocol::PlatoonSpeed> platoonSpeed{false};
- TimedAtomicValue<platoonProtocol::InnerPlatoonDistance> innerPlatoonDistance{false};
- TimedAtomicValue<float> speed{false};
+ networking::time::TimedAtomicValue<bool> loggingEnabled{false};
+ networking::time::TimedAtomicValue<bool> platoonEnabled{false};
+ networking::time::TimedAtomicValue<bool> rcModeEnabled{false};
+ networking::time::TimedAtomicValue<platoonProtocol::PlatoonSpeed> platoonSpeed{false};
+ networking::time::TimedAtomicValue<platoonProtocol::InnerPlatoonDistance> innerPlatoonDistance{false};
+ networking::time::TimedAtomicValue<float> speed{false};
void receiveCommand();
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/.gitignore b/modules/catkin_ws/src/VeloxProtocolLib/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..1b72d0afb239e12b614b051f428b69409184cfb5
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/.gitignore
@@ -0,0 +1,2 @@
+cmake-build-debug
+.idea
\ No newline at end of file
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt b/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ae61916e7b74f2ed52baa4fe7d848eac5fe97291
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(VeloxProtocolLib)
+
+set(CMAKE_CXX_STANDARD 14)
+
+set(CMAKE_CXX_FLAGS -pthread)
+
+set(SOURCE_FILES
+ include/mavlink/checksum.h
+ include/mavlink/mavlink_conversions.h
+ include/mavlink/mavlink_get_info.h
+ include/mavlink/mavlink_helpers.h
+ include/mavlink/mavlink_sha256.h
+ include/mavlink/mavlink_types.h
+ include/mavlink/protocol.h
+ include/mavlink/velox/mavlink.h
+ include/mavlink/velox/mavlink_msg_carcontrol.h
+ include/mavlink/velox/mavlink_msg_cmd_request_clocksync.h
+ include/mavlink/velox/mavlink_msg_cmd_request_msg.h
+ include/mavlink/velox/mavlink_msg_cmd_request_statechange.h
+ include/mavlink/velox/mavlink_msg_error.h
+ include/mavlink/velox/mavlink_msg_heartbeat.h
+ include/mavlink/velox/mavlink_msg_odometry.h
+ include/mavlink/velox/mavlink_msg_trajectory.h
+ include/mavlink/velox/testsuite.h
+ include/mavlink/velox/velox.h
+ include/mavlink/velox/version.h
+ include/serial_port.h
+ include/Velox.h
+ src/serial_port.cpp
+ src/Velox.cpp)
+
+add_executable(main ${SOURCE_FILES} test/Main.cpp)
\ No newline at end of file
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/Velox.h b/modules/catkin_ws/src/VeloxProtocolLib/include/Velox.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc4fe2a1393a332d4754ed11c000c9313daa5bee
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/Velox.h
@@ -0,0 +1,21 @@
+//
+// Created by philipp on 20.04.18.
+//
+
+#ifndef MAVLINK_ODROID_VELOX_H
+#define MAVLINK_ODROID_VELOX_H
+
+namespace veloxProtocol
+{
+
+class Client
+{
+public:
+
+
+private:
+};
+
+}
+
+#endif //MAVLINK_ODROID_VELOX_H
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/checksum.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/checksum.h
new file mode 100755
index 0000000000000000000000000000000000000000..2bb0df24e3c34c002dba8db11f6622453f813dca
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/checksum.h
@@ -0,0 +1,95 @@
+#pragma once
+
+#if defined(MAVLINK_USE_CXX_NAMESPACE)
+namespace mavlink {
+#elif defined(__cplusplus)
+extern "C" {
+#endif
+
+// Visual Studio versions before 2010 don't have stdint.h, so we just error out.
+#if (defined _MSC_VER) && (_MSC_VER < 1600)
+#error "The C-MAVLink implementation requires Visual Studio 2010 or greater"
+#endif
+
+#include <stdint.h>
+
+/**
+ *
+ * CALCULATE THE CHECKSUM
+ *
+ */
+
+#define X25_INIT_CRC 0xffff
+#define X25_VALIDATE_CRC 0xf0b8
+
+#ifndef HAVE_CRC_ACCUMULATE
+/**
+ * @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);
+}
+#endif
+
+
+/**
+ * @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(const uint8_t* pBuffer, uint16_t length)
+{
+ uint16_t crcTmp;
+ crc_init(&crcTmp);
+ while (length--) {
+ crc_accumulate(*pBuffer++, &crcTmp);
+ }
+ return crcTmp;
+}
+
+
+/**
+ * @brief Accumulate the X.25 CRC by adding an array of bytes
+ *
+ * The checksum function adds the hash of one char at a time to the
+ * 16 bit checksum (uint16_t).
+ *
+ * @param data new bytes to hash
+ * @param crcAccum the already accumulated checksum
+ **/
+static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
+{
+ const uint8_t *p = (const uint8_t *)pBuffer;
+ while (length--) {
+ crc_accumulate(*p++, crcAccum);
+ }
+}
+
+#if defined(MAVLINK_USE_CXX_NAMESPACE) || defined(__cplusplus)
+}
+#endif
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_conversions.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_conversions.h
new file mode 100755
index 0000000000000000000000000000000000000000..9baee264b7841f1052248b17162e2dab63023b6d
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_conversions.h
@@ -0,0 +1,209 @@
+#pragma once
+
+/* enable math defines on Windows */
+#ifdef _MSC_VER
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+#endif
+#include <math.h>
+
+#ifndef M_PI_2
+ #define M_PI_2 ((float)asin(1))
+#endif
+
+/**
+ * @file mavlink_conversions.h
+ *
+ * These conversion functions follow the NASA rotation standards definition file
+ * available online.
+ *
+ * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
+ * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
+ * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
+ * protocol as widely as possible.
+ *
+ * @author James Goppert
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+/**
+ * Converts a quaternion to a rotation matrix
+ *
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ * @param dcm a 3x3 rotation matrix
+ */
+MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
+{
+ double a = quaternion[0];
+ double b = quaternion[1];
+ double c = quaternion[2];
+ double d = quaternion[3];
+ double aSq = a * a;
+ double bSq = b * b;
+ double cSq = c * c;
+ double dSq = d * d;
+ dcm[0][0] = aSq + bSq - cSq - dSq;
+ dcm[0][1] = 2 * (b * c - a * d);
+ dcm[0][2] = 2 * (a * c + b * d);
+ dcm[1][0] = 2 * (b * c + a * d);
+ dcm[1][1] = aSq - bSq + cSq - dSq;
+ dcm[1][2] = 2 * (c * d - a * b);
+ dcm[2][0] = 2 * (b * d - a * c);
+ dcm[2][1] = 2 * (a * b + c * d);
+ dcm[2][2] = aSq - bSq - cSq + dSq;
+}
+
+
+/**
+ * Converts a rotation matrix to euler angles
+ *
+ * @param dcm a 3x3 rotation matrix
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ */
+MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
+{
+ float phi, theta, psi;
+ theta = asin(-dcm[2][0]);
+
+ if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
+ phi = 0.0f;
+ psi = (atan2f(dcm[1][2] - dcm[0][1],
+ dcm[0][2] + dcm[1][1]) + phi);
+
+ } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
+ phi = 0.0f;
+ psi = atan2f(dcm[1][2] - dcm[0][1],
+ dcm[0][2] + dcm[1][1] - phi);
+
+ } else {
+ phi = atan2f(dcm[2][1], dcm[2][2]);
+ psi = atan2f(dcm[1][0], dcm[0][0]);
+ }
+
+ *roll = phi;
+ *pitch = theta;
+ *yaw = psi;
+}
+
+
+/**
+ * Converts a quaternion to euler angles
+ *
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ */
+MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
+{
+ float dcm[3][3];
+ mavlink_quaternion_to_dcm(quaternion, dcm);
+ mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);
+}
+
+
+/**
+ * Converts euler angles to a quaternion
+ *
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ */
+MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
+{
+ float cosPhi_2 = cosf(roll / 2);
+ float sinPhi_2 = sinf(roll / 2);
+ float cosTheta_2 = cosf(pitch / 2);
+ float sinTheta_2 = sinf(pitch / 2);
+ float cosPsi_2 = cosf(yaw / 2);
+ float sinPsi_2 = sinf(yaw / 2);
+ quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
+ sinPhi_2 * sinTheta_2 * sinPsi_2);
+ quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
+ cosPhi_2 * sinTheta_2 * sinPsi_2);
+ quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
+ sinPhi_2 * cosTheta_2 * sinPsi_2);
+ quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
+ sinPhi_2 * sinTheta_2 * cosPsi_2);
+}
+
+
+/**
+ * Converts a rotation matrix to a quaternion
+ * Reference:
+ * - Shoemake, Quaternions,
+ * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
+ *
+ * @param dcm a 3x3 rotation matrix
+ * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
+ */
+MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
+{
+ float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
+ if (tr > 0.0f) {
+ float s = sqrtf(tr + 1.0f);
+ quaternion[0] = s * 0.5f;
+ s = 0.5f / s;
+ quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
+ quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
+ quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
+ } else {
+ /* Find maximum diagonal element in dcm
+ * store index in dcm_i */
+ int dcm_i = 0;
+ int i;
+ for (i = 1; i < 3; i++) {
+ if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
+ dcm_i = i;
+ }
+ }
+
+ int dcm_j = (dcm_i + 1) % 3;
+ int dcm_k = (dcm_i + 2) % 3;
+
+ float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
+ dcm[dcm_k][dcm_k]) + 1.0f);
+ quaternion[dcm_i + 1] = s * 0.5f;
+ s = 0.5f / s;
+ quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
+ quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
+ quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
+ }
+}
+
+
+/**
+ * Converts euler angles to a rotation matrix
+ *
+ * @param roll the roll angle in radians
+ * @param pitch the pitch angle in radians
+ * @param yaw the yaw angle in radians
+ * @param dcm a 3x3 rotation matrix
+ */
+MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
+{
+ float cosPhi = cosf(roll);
+ float sinPhi = sinf(roll);
+ float cosThe = cosf(pitch);
+ float sinThe = sinf(pitch);
+ float cosPsi = cosf(yaw);
+ float sinPsi = sinf(yaw);
+
+ dcm[0][0] = cosThe * cosPsi;
+ dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+ dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+ dcm[1][0] = cosThe * sinPsi;
+ dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+ dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+ dcm[2][0] = -sinThe;
+ dcm[2][1] = sinPhi * cosThe;
+ dcm[2][2] = cosPhi * cosThe;
+}
+
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_get_info.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_get_info.h
new file mode 100755
index 0000000000000000000000000000000000000000..1118105895406145a10557ee7ee40e53e45563fa
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_get_info.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#ifdef MAVLINK_USE_MESSAGE_INFO
+#define MAVLINK_HAVE_GET_MESSAGE_INFO
+/*
+ return the message_info struct for a message
+*/
+MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg)
+{
+ static const mavlink_message_info_t mavlink_message_info[] = MAVLINK_MESSAGE_INFO;
+ /*
+ use a bisection search to find the right entry. A perfect hash may be better
+ Note that this assumes the table is sorted with primary key msgid
+ */
+ uint32_t msgid = msg->msgid;
+ uint32_t low=0, high=sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]);
+ while (low < high) {
+ uint32_t mid = (low+1+high)/2;
+ if (msgid < mavlink_message_info[mid].msgid) {
+ high = mid-1;
+ continue;
+ }
+ if (msgid > mavlink_message_info[mid].msgid) {
+ low = mid;
+ continue;
+ }
+ low = mid;
+ break;
+ }
+ if (mavlink_message_info[low].msgid == msgid) {
+ return &mavlink_message_info[low];
+ }
+ return NULL;
+}
+#endif // MAVLINK_USE_MESSAGE_INFO
+
+
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_helpers.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_helpers.h
new file mode 100755
index 0000000000000000000000000000000000000000..0107cc153e46a3ebec48ae7a10731d0560f71a56
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_helpers.h
@@ -0,0 +1,1119 @@
+#pragma once
+
+#include "string.h"
+#include "checksum.h"
+#include "mavlink_types.h"
+#include "mavlink_conversions.h"
+#include <stdio.h>
+
+#ifndef MAVLINK_HELPER
+#define MAVLINK_HELPER
+#endif
+
+#include "mavlink_sha256.h"
+
+#ifdef MAVLINK_USE_CXX_NAMESPACE
+namespace mavlink {
+#endif
+
+/*
+ * Internal function to give access to the channel status for each channel
+ */
+#ifndef MAVLINK_GET_CHANNEL_STATUS
+MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
+{
+#ifdef MAVLINK_EXTERNAL_RX_STATUS
+ // No m_mavlink_status array defined in function,
+ // has to be defined externally
+#else
+ static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
+#endif
+ return &m_mavlink_status[chan];
+}
+#endif
+
+/*
+ * Internal function to give access to the channel buffer for each channel
+ */
+#ifndef MAVLINK_GET_CHANNEL_BUFFER
+MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
+{
+
+#ifdef MAVLINK_EXTERNAL_RX_BUFFER
+ // No m_mavlink_buffer array defined in function,
+ // has to be defined externally
+#else
+ static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
+#endif
+ return &m_mavlink_buffer[chan];
+}
+#endif // MAVLINK_GET_CHANNEL_BUFFER
+
+/**
+ * @brief Reset the status of a channel.
+ */
+MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
+{
+ mavlink_status_t *status = mavlink_get_channel_status(chan);
+ status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+}
+
+/**
+ * @brief create a signature block for a packet
+ */
+MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
+ uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
+ const uint8_t *header, uint8_t header_len,
+ const uint8_t *packet, uint8_t packet_len,
+ const uint8_t crc[2])
+{
+ mavlink_sha256_ctx ctx;
+ union {
+ uint64_t t64;
+ uint8_t t8[8];
+ } tstamp;
+ if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
+ return 0;
+ }
+ signature[0] = signing->link_id;
+ tstamp.t64 = signing->timestamp;
+ memcpy(&signature[1], tstamp.t8, 6);
+ signing->timestamp++;
+
+ mavlink_sha256_init(&ctx);
+ mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
+ mavlink_sha256_update(&ctx, header, header_len);
+ mavlink_sha256_update(&ctx, packet, packet_len);
+ mavlink_sha256_update(&ctx, crc, 2);
+ mavlink_sha256_update(&ctx, signature, 7);
+ mavlink_sha256_final_48(&ctx, &signature[7]);
+
+ return MAVLINK_SIGNATURE_BLOCK_LEN;
+}
+
+/**
+ * return new packet length for trimming payload of any trailing zero
+ * bytes. Used in MAVLink2 to give simple support for variable length
+ * arrays.
+ */
+MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length)
+{
+ while (length > 1 && payload[length-1] == 0) {
+ length--;
+ }
+ return length;
+}
+
+/**
+ * @brief check a signature block for a packet
+ */
+MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing,
+ mavlink_signing_streams_t *signing_streams,
+ const mavlink_message_t *msg)
+{
+ if (signing == NULL) {
+ return true;
+ }
+ const uint8_t *p = (const uint8_t *)&msg->magic;
+ const uint8_t *psig = msg->signature;
+ const uint8_t *incoming_signature = psig+7;
+ mavlink_sha256_ctx ctx;
+ uint8_t signature[6];
+ uint16_t i;
+
+ mavlink_sha256_init(&ctx);
+ mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
+ mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len);
+ mavlink_sha256_update(&ctx, msg->ck, 2);
+ mavlink_sha256_update(&ctx, psig, 1+6);
+ mavlink_sha256_final_48(&ctx, signature);
+ if (memcmp(signature, incoming_signature, 6) != 0) {
+ return false;
+ }
+
+ // now check timestamp
+ union tstamp {
+ uint64_t t64;
+ uint8_t t8[8];
+ } tstamp;
+ uint8_t link_id = psig[0];
+ tstamp.t64 = 0;
+ memcpy(tstamp.t8, psig+1, 6);
+
+ if (signing_streams == NULL) {
+ return false;
+ }
+
+ // find stream
+ for (i=0; i<signing_streams->num_signing_streams; i++) {
+ if (msg->sysid == signing_streams->stream[i].sysid &&
+ msg->compid == signing_streams->stream[i].compid &&
+ link_id == signing_streams->stream[i].link_id) {
+ break;
+ }
+ }
+ if (i == signing_streams->num_signing_streams) {
+ if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) {
+ // over max number of streams
+ return false;
+ }
+ // new stream. Only accept if timestamp is not more than 1 minute old
+ if (tstamp.t64 + 6000*1000UL < signing->timestamp) {
+ return false;
+ }
+ // add new stream
+ signing_streams->stream[i].sysid = msg->sysid;
+ signing_streams->stream[i].compid = msg->compid;
+ signing_streams->stream[i].link_id = link_id;
+ signing_streams->num_signing_streams++;
+ } else {
+ union tstamp last_tstamp;
+ last_tstamp.t64 = 0;
+ memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6);
+ if (tstamp.t64 <= last_tstamp.t64) {
+ // repeating old timestamp
+ return false;
+ }
+ }
+
+ // remember last timestamp
+ memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6);
+
+ // our next timestamp must be at least this timestamp
+ if (tstamp.t64 > signing->timestamp) {
+ signing->timestamp = tstamp.t64;
+ }
+ return true;
+}
+
+
+/**
+ * @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
+ */
+MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
+ mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
+{
+ bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
+ bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
+ uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
+ uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
+ uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
+ if (mavlink1) {
+ msg->magic = MAVLINK_STX_MAVLINK1;
+ header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
+ } else {
+ msg->magic = MAVLINK_STX;
+ }
+ msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
+ msg->sysid = system_id;
+ msg->compid = component_id;
+ msg->incompat_flags = 0;
+ if (signing) {
+ msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
+ }
+ msg->compat_flags = 0;
+ msg->seq = status->current_tx_seq;
+ status->current_tx_seq = status->current_tx_seq + 1;
+
+ // form the header as a byte array for the crc
+ buf[0] = msg->magic;
+ buf[1] = msg->len;
+ if (mavlink1) {
+ buf[2] = msg->seq;
+ buf[3] = msg->sysid;
+ buf[4] = msg->compid;
+ buf[5] = msg->msgid & 0xFF;
+ } else {
+ buf[2] = msg->incompat_flags;
+ buf[3] = msg->compat_flags;
+ buf[4] = msg->seq;
+ buf[5] = msg->sysid;
+ buf[6] = msg->compid;
+ buf[7] = msg->msgid & 0xFF;
+ buf[8] = (msg->msgid >> 8) & 0xFF;
+ buf[9] = (msg->msgid >> 16) & 0xFF;
+ }
+
+ msg->checksum = crc_calculate(&buf[1], header_len-1);
+ crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
+ crc_accumulate(crc_extra, &msg->checksum);
+ mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
+ mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
+
+ if (signing) {
+ mavlink_sign_packet(status->signing,
+ msg->signature,
+ (const uint8_t *)buf, header_len,
+ (const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
+ (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
+ }
+
+ return msg->len + header_len + 2 + signature_len;
+}
+
+MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
+ uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
+{
+ mavlink_status_t *status = mavlink_get_channel_status(chan);
+ return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra);
+}
+
+/**
+ * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
+ */
+MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
+ uint8_t min_length, uint8_t length, uint8_t crc_extra)
+{
+ return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
+}
+
+static inline void _mav_parse_error(mavlink_status_t *status)
+{
+ status->parse_error++;
+}
+
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
+
+/**
+ * @brief Finalize a MAVLink message with channel assignment and send
+ */
+MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,
+ const char *packet,
+ uint8_t min_length, uint8_t length, uint8_t crc_extra)
+{
+ uint16_t checksum;
+ uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
+ uint8_t ck[2];
+ mavlink_status_t *status = mavlink_get_channel_status(chan);
+ uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
+ uint8_t signature_len = 0;
+ uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
+ bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
+ bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
+
+ if (mavlink1) {
+ length = min_length;
+ if (msgid > 255) {
+ // can't send 16 bit messages
+ _mav_parse_error(status);
+ return;
+ }
+ header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
+ buf[0] = MAVLINK_STX_MAVLINK1;
+ buf[1] = length;
+ buf[2] = status->current_tx_seq;
+ buf[3] = mavlink_system.sysid;
+ buf[4] = mavlink_system.compid;
+ buf[5] = msgid & 0xFF;
+ } else {
+ uint8_t incompat_flags = 0;
+ if (signing) {
+ incompat_flags |= MAVLINK_IFLAG_SIGNED;
+ }
+ length = _mav_trim_payload(packet, length);
+ buf[0] = MAVLINK_STX;
+ buf[1] = length;
+ buf[2] = incompat_flags;
+ buf[3] = 0; // compat_flags
+ buf[4] = status->current_tx_seq;
+ buf[5] = mavlink_system.sysid;
+ buf[6] = mavlink_system.compid;
+ buf[7] = msgid & 0xFF;
+ buf[8] = (msgid >> 8) & 0xFF;
+ buf[9] = (msgid >> 16) & 0xFF;
+ }
+ status->current_tx_seq++;
+ checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
+ crc_accumulate_buffer(&checksum, packet, length);
+ crc_accumulate(crc_extra, &checksum);
+ ck[0] = (uint8_t)(checksum & 0xFF);
+ ck[1] = (uint8_t)(checksum >> 8);
+
+ if (signing) {
+ // possibly add a signature
+ signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,
+ (const uint8_t *)packet, length, ck);
+ }
+
+ MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
+ _mavlink_send_uart(chan, (const char *)buf, header_len+1);
+ _mavlink_send_uart(chan, packet, length);
+ _mavlink_send_uart(chan, (const char *)ck, 2);
+ if (signature_len != 0) {
+ _mavlink_send_uart(chan, (const char *)signature, signature_len);
+ }
+ MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
+}
+
+/**
+ * @brief re-send a message over a uart channel
+ * this is more stack efficient than re-marshalling the message
+ * If the message is signed then the original signature is also sent
+ */
+MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
+{
+ uint8_t ck[2];
+
+ ck[0] = (uint8_t)(msg->checksum & 0xFF);
+ ck[1] = (uint8_t)(msg->checksum >> 8);
+ // XXX use the right sequence here
+
+ uint8_t header_len;
+ uint8_t signature_len;
+
+ if (msg->magic == MAVLINK_STX_MAVLINK1) {
+ header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
+ signature_len = 0;
+ MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
+ // we can't send the structure directly as it has extra mavlink2 elements in it
+ uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1];
+ buf[0] = msg->magic;
+ buf[1] = msg->len;
+ buf[2] = msg->seq;
+ buf[3] = msg->sysid;
+ buf[4] = msg->compid;
+ buf[5] = msg->msgid & 0xFF;
+ _mavlink_send_uart(chan, (const char*)buf, header_len);
+ } else {
+ header_len = MAVLINK_CORE_HEADER_LEN + 1;
+ signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
+ MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
+ uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1];
+ buf[0] = msg->magic;
+ buf[1] = msg->len;
+ buf[2] = msg->incompat_flags;
+ buf[3] = msg->compat_flags;
+ buf[4] = msg->seq;
+ buf[5] = msg->sysid;
+ buf[6] = msg->compid;
+ buf[7] = msg->msgid & 0xFF;
+ buf[8] = (msg->msgid >> 8) & 0xFF;
+ buf[9] = (msg->msgid >> 16) & 0xFF;
+ _mavlink_send_uart(chan, (const char *)buf, header_len);
+ }
+ _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
+ _mavlink_send_uart(chan, (const char *)ck, 2);
+ if (signature_len != 0) {
+ _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN);
+ }
+ MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
+}
+#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+/**
+ * @brief Pack a message to send it over a serial byte stream
+ */
+MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
+{
+ uint8_t signature_len, header_len;
+ uint8_t *ck;
+ uint8_t length = msg->len;
+
+ if (msg->magic == MAVLINK_STX_MAVLINK1) {
+ signature_len = 0;
+ header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
+ buf[0] = msg->magic;
+ buf[1] = length;
+ buf[2] = msg->seq;
+ buf[3] = msg->sysid;
+ buf[4] = msg->compid;
+ buf[5] = msg->msgid & 0xFF;
+ memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
+ ck = buf + header_len + 1 + (uint16_t)msg->len;
+ } else {
+ length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
+ header_len = MAVLINK_CORE_HEADER_LEN;
+ buf[0] = msg->magic;
+ buf[1] = length;
+ buf[2] = msg->incompat_flags;
+ buf[3] = msg->compat_flags;
+ buf[4] = msg->seq;
+ buf[5] = msg->sysid;
+ buf[6] = msg->compid;
+ buf[7] = msg->msgid & 0xFF;
+ buf[8] = (msg->msgid >> 8) & 0xFF;
+ buf[9] = (msg->msgid >> 16) & 0xFF;
+ memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
+ ck = buf + header_len + 1 + (uint16_t)length;
+ signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
+ }
+ ck[0] = (uint8_t)(msg->checksum & 0xFF);
+ ck[1] = (uint8_t)(msg->checksum >> 8);
+ if (signature_len > 0) {
+ memcpy(&ck[2], msg->signature, signature_len);
+ }
+
+ return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
+}
+
+union __mavlink_bitfield {
+ uint8_t uint8;
+ int8_t int8;
+ uint16_t uint16;
+ int16_t int16;
+ uint32_t uint32;
+ int32_t int32;
+};
+
+
+MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
+{
+ crc_init(&msg->checksum);
+}
+
+MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
+{
+ crc_accumulate(c, &msg->checksum);
+}
+
+/*
+ return the crc_entry value for a msgid
+*/
+#ifndef MAVLINK_GET_MSG_ENTRY
+MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
+{
+ static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
+ /*
+ use a bisection search to find the right entry. A perfect hash may be better
+ Note that this assumes the table is sorted by msgid
+ */
+ uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]);
+ while (low < high) {
+ uint32_t mid = (low+1+high)/2;
+ if (msgid < mavlink_message_crcs[mid].msgid) {
+ high = mid-1;
+ continue;
+ }
+ if (msgid > mavlink_message_crcs[mid].msgid) {
+ low = mid;
+ continue;
+ }
+ low = mid;
+ break;
+ }
+ if (mavlink_message_crcs[low].msgid != msgid) {
+ // msgid is not in the table
+ return NULL;
+ }
+ return &mavlink_message_crcs[low];
+}
+#endif // MAVLINK_GET_MSG_ENTRY
+
+/*
+ return the crc_extra value for a message
+*/
+MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg)
+{
+ const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
+ return e?e->crc_extra:0;
+}
+
+/*
+ return the expected message length
+*/
+#define MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH
+MAVLINK_HELPER uint8_t mavlink_expected_message_length(const mavlink_message_t *msg)
+{
+ const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
+ return e?e->msg_len:0;
+}
+
+/**
+ * This is a varient of mavlink_frame_char() but with caller supplied
+ * parsing buffers. It is useful when you want to create a MAVLink
+ * parser in a library that doesn't use any global variables
+ *
+ * @param rxmsg parsing message buffer
+ * @param status parsing starus buffer
+ * @param c The char to parse
+ *
+ * @param returnMsg NULL if no message could be decoded, the message data else
+ * @param returnStats if a message was decoded, this is filled with the channel's stats
+ * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
+ *
+ * A typical use scenario of this function call is:
+ *
+ * @code
+ * #include <mavlink.h>
+ *
+ * mavlink_message_t msg;
+ * int chan = 0;
+ *
+ *
+ * while(serial.bytesAvailable > 0)
+ * {
+ * uint8_t byte = serial.getNextByte();
+ * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
+ * {
+ * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
+ * }
+ * }
+ *
+ *
+ * @endcode
+ */
+MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
+ mavlink_status_t* status,
+ uint8_t c,
+ mavlink_message_t* r_message,
+ mavlink_status_t* r_mavlink_status)
+{
+ /* Enable this option to check the length of each message.
+ This allows invalid messages to be caught much sooner. Use if the transmission
+ medium is prone to missing (or extra) characters (e.g. a radio that fades in
+ and out). Only use if the channel will only contain messages types listed in
+ the headers.
+ */
+#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
+#ifndef MAVLINK_MESSAGE_LENGTH
+ static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
+#endif
+#endif
+
+ int bufferIndex = 0;
+
+ status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
+
+ switch (status->parse_state)
+ {
+ case MAVLINK_PARSE_STATE_UNINIT:
+ case MAVLINK_PARSE_STATE_IDLE:
+ if (c == MAVLINK_STX)
+ {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
+ rxmsg->len = 0;
+ rxmsg->magic = c;
+ status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
+ mavlink_start_checksum(rxmsg);
+ } else if (c == MAVLINK_STX_MAVLINK1)
+ {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
+ rxmsg->len = 0;
+ rxmsg->magic = c;
+ status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
+ mavlink_start_checksum(rxmsg);
+ }
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_STX:
+ if (status->msg_received
+/* Support shorter buffers than the
+ default maximum packet size */
+#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
+ || c > MAVLINK_MAX_PAYLOAD_LEN
+#endif
+ )
+ {
+ status->buffer_overrun++;
+ _mav_parse_error(status);
+ 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);
+ if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
+ rxmsg->incompat_flags = 0;
+ rxmsg->compat_flags = 0;
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
+ } else {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
+ }
+ }
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_LENGTH:
+ rxmsg->incompat_flags = c;
+ if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
+ // message includes an incompatible feature flag
+ _mav_parse_error(status);
+ status->msg_received = 0;
+ status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+ break;
+ }
+ mavlink_update_checksum(rxmsg, c);
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
+ rxmsg->compat_flags = c;
+ mavlink_update_checksum(rxmsg, c);
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
+ 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 (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
+#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
+ if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
+ {
+ _mav_parse_error(status);
+ status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+ break;
+ }
+#endif
+ } else {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
+ }
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_MSGID1:
+ rxmsg->msgid |= c<<8;
+ mavlink_update_checksum(rxmsg, c);
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_MSGID2:
+ rxmsg->msgid |= c<<16;
+ mavlink_update_checksum(rxmsg, c);
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
+#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
+ if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
+ {
+ _mav_parse_error(status);
+ status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+ break;
+ }
+#endif
+ break;
+
+ case MAVLINK_PARSE_STATE_GOT_MSGID3:
+ _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)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: {
+ const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
+ uint8_t crc_extra = e?e->crc_extra:0;
+ mavlink_update_checksum(rxmsg, crc_extra);
+ if (c != (rxmsg->checksum & 0xFF)) {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
+ } else {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
+ }
+ rxmsg->ck[0] = c;
+
+ // zero-fill the packet to cope with short incoming packets
+ if (e && status->packet_idx < e->msg_len) {
+ memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->msg_len - status->packet_idx);
+ }
+ break;
+ }
+
+ case MAVLINK_PARSE_STATE_GOT_CRC1:
+ case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
+ if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
+ // got a bad CRC message
+ status->msg_received = MAVLINK_FRAMING_BAD_CRC;
+ } else {
+ // Successfully got message
+ status->msg_received = MAVLINK_FRAMING_OK;
+ }
+ rxmsg->ck[1] = c;
+
+ if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
+ status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
+ status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
+
+ // If the CRC is already wrong, don't overwrite msg_received,
+ // otherwise we can end up with garbage flagged as valid.
+ if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
+ status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
+ }
+ } else {
+ if (status->signing &&
+ (status->signing->accept_unsigned_callback == NULL ||
+ !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
+
+ // If the CRC is already wrong, don't overwrite msg_received.
+ if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
+ status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
+ }
+ }
+ status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+ memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
+ }
+ break;
+ case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
+ rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
+ status->signature_wait--;
+ if (status->signature_wait == 0) {
+ // we have the whole signature, check it is OK
+ bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
+ if (!sig_ok &&
+ (status->signing->accept_unsigned_callback &&
+ status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
+ // accepted via application level override
+ sig_ok = true;
+ }
+ if (sig_ok) {
+ status->msg_received = MAVLINK_FRAMING_OK;
+ } else {
+ status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
+ }
+ 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 == MAVLINK_FRAMING_OK)
+ {
+ //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_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
+ r_mavlink_status->parse_state = status->parse_state;
+ r_mavlink_status->packet_idx = status->packet_idx;
+ 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;
+ r_mavlink_status->flags = status->flags;
+ status->parse_error = 0;
+
+ if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
+ /*
+ the CRC came out wrong. We now need to overwrite the
+ msg CRC with the one on the wire so that if the
+ caller decides to forward the message anyway that
+ mavlink_msg_to_send_buffer() won't overwrite the
+ checksum
+ */
+ r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
+ }
+
+ return status->msg_received;
+}
+
+/**
+ * This is a convenience function which handles the complete MAVLink parsing.
+ * the function will parse one byte at a time and return the complete packet once
+ * it could be successfully decoded. This function will return 0, 1 or
+ * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
+ *
+ * Messages are parsed into an internal buffer (one for each channel). When a complete
+ * message is received it is copies into *returnMsg and the channel's status is
+ * copied into *returnStats.
+ *
+ * @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 parse
+ *
+ * @param returnMsg NULL if no message could be decoded, the message data else
+ * @param returnStats if a message was decoded, this is filled with the channel's stats
+ * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
+ *
+ * A typical use scenario of this function call is:
+ *
+ * @code
+ * #include <mavlink.h>
+ *
+ * mavlink_message_t msg;
+ * int chan = 0;
+ *
+ *
+ * while(serial.bytesAvailable > 0)
+ * {
+ * uint8_t byte = serial.getNextByte();
+ * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
+ * {
+ * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
+ * }
+ * }
+ *
+ *
+ * @endcode
+ */
+MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
+{
+ return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
+ mavlink_get_channel_status(chan),
+ c,
+ r_message,
+ r_mavlink_status);
+}
+
+/**
+ * Set the protocol version
+ */
+MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version)
+{
+ mavlink_status_t *status = mavlink_get_channel_status(chan);
+ if (version > 1) {
+ status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+ } else {
+ status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+ }
+}
+
+/**
+ * Get the protocol version
+ *
+ * @return 1 for v1, 2 for v2
+ */
+MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan)
+{
+ mavlink_status_t *status = mavlink_get_channel_status(chan);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) {
+ return 1;
+ } else {
+ return 2;
+ }
+}
+
+/**
+ * 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. This function will return 0 or 1.
+ *
+ * Messages are parsed into an internal buffer (one for each channel). When a complete
+ * message is received it is copies into *returnMsg and the channel's status is
+ * copied into *returnStats.
+ *
+ * @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 parse
+ *
+ * @param returnMsg NULL if no message could be decoded, the message data else
+ * @param returnStats if a message was decoded, this is filled with the channel's stats
+ * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
+ *
+ * A typical use scenario of this function call is:
+ *
+ * @code
+ * #include <mavlink.h>
+ *
+ * mavlink_message_t msg;
+ * int chan = 0;
+ *
+ *
+ * while(serial.bytesAvailable > 0)
+ * {
+ * uint8_t byte = serial.getNextByte();
+ * if (mavlink_parse_char(chan, byte, &msg))
+ * {
+ * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
+ * }
+ * }
+ *
+ *
+ * @endcode
+ */
+MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
+{
+ uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
+ if (msg_received == MAVLINK_FRAMING_BAD_CRC ||
+ msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {
+ // we got a bad CRC. Treat as a parse failure
+ mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
+ mavlink_status_t* status = mavlink_get_channel_status(chan);
+ _mav_parse_error(status);
+ status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
+ status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+ if (c == MAVLINK_STX)
+ {
+ status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
+ rxmsg->len = 0;
+ mavlink_start_checksum(rxmsg);
+ }
+ return 0;
+ }
+ return msg_received;
+}
+
+/**
+ * @brief Put a bitfield of length 1-32 bit into the buffer
+ *
+ * @param b the value to add, will be encoded in the bitfield
+ * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
+ * @param packet_index the position in the packet (the index of the first byte to use)
+ * @param bit_index the position in the byte (the index of the first bit to use)
+ * @param buffer packet buffer to write into
+ * @return new position of the last used byte in the buffer
+ */
+MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
+{
+ uint16_t bits_remain = bits;
+ // Transform number into network order
+ int32_t v;
+ uint8_t i_bit_index, i_byte_index, curr_bits_n;
+#if MAVLINK_NEED_BYTE_SWAP
+ union {
+ int32_t i;
+ uint8_t b[4];
+ } bin, bout;
+ bin.i = b;
+ bout.b[0] = bin.b[3];
+ bout.b[1] = bin.b[2];
+ bout.b[2] = bin.b[1];
+ bout.b[3] = bin.b[0];
+ v = bout.i;
+#else
+ v = b;
+#endif
+
+ // buffer in
+ // 01100000 01000000 00000000 11110001
+ // buffer out
+ // 11110001 00000000 01000000 01100000
+
+ // Existing partly filled byte (four free slots)
+ // 0111xxxx
+
+ // Mask n free bits
+ // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
+ // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
+
+ // Shift n bits into the right position
+ // out = in >> n;
+
+ // Mask and shift bytes
+ i_bit_index = bit_index;
+ i_byte_index = packet_index;
+ if (bit_index > 0)
+ {
+ // If bits were available at start, they were available
+ // in the byte before the current index
+ i_byte_index--;
+ }
+
+ // While bits have not been packed yet
+ while (bits_remain > 0)
+ {
+ // Bits still have to be packed
+ // there can be more than 8 bits, so
+ // we might have to pack them into more than one byte
+
+ // First pack everything we can into the current 'open' byte
+ //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
+ //FIXME
+ if (bits_remain <= (uint8_t)(8 - i_bit_index))
+ {
+ // Enough space
+ curr_bits_n = (uint8_t)bits_remain;
+ }
+ else
+ {
+ curr_bits_n = (8 - i_bit_index);
+ }
+
+ // Pack these n bits into the current byte
+ // Mask out whatever was at that position with ones (xxx11111)
+ buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
+ // Put content to this position, by masking out the non-used part
+ buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
+
+ // Increment the bit index
+ i_bit_index += curr_bits_n;
+
+ // Now proceed to the next byte, if necessary
+ bits_remain -= curr_bits_n;
+ if (bits_remain > 0)
+ {
+ // Offer another 8 bits / one byte
+ i_byte_index++;
+ i_bit_index = 0;
+ }
+ }
+
+ *r_bit_index = i_bit_index;
+ // If a partly filled byte is present, mark this as consumed
+ if (i_bit_index != 7) i_byte_index++;
+ return i_byte_index - packet_index;
+}
+
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+// To make MAVLink work on your MCU, define comm_send_ch() if you wish
+// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
+// whole packet at a time
+
+/*
+
+#include "mavlink_types.h"
+
+void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
+{
+ if (chan == MAVLINK_COMM_0)
+ {
+ uart0_transmit(ch);
+ }
+ if (chan == MAVLINK_COMM_1)
+ {
+ uart1_transmit(ch);
+ }
+}
+ */
+
+MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
+{
+#ifdef MAVLINK_SEND_UART_BYTES
+ /* this is the more efficient approach, if the platform
+ defines it */
+ MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
+#else
+ /* fallback to one byte at a time */
+ uint16_t i;
+ for (i = 0; i < len; i++) {
+ comm_send_ch(chan, (uint8_t)buf[i]);
+ }
+#endif
+}
+#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+#ifdef MAVLINK_USE_CXX_NAMESPACE
+} // namespace mavlink
+#endif
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_sha256.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_sha256.h
new file mode 100755
index 0000000000000000000000000000000000000000..317feb9e1874bdace0533cfaf3d06e7cb956781d
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_sha256.h
@@ -0,0 +1,252 @@
+#pragma once
+
+/*
+ sha-256 implementation for MAVLink based on Heimdal sources, with
+ modifications to suit mavlink headers
+ */
+/*
+ * Copyright (c) 1995 - 2001 Kungliga Tekniska Högskolan
+ * (Royal Institute of Technology, Stockholm, Sweden).
+ * All rights reserved.
+ *
+ * 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. Neither the name of the Institute nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``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 INSTITUTE OR CONTRIBUTORS 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.
+ */
+
+/*
+ allow implementation to provide their own sha256 with the same API
+*/
+#ifndef HAVE_MAVLINK_SHA256
+
+#ifdef MAVLINK_USE_CXX_NAMESPACE
+namespace mavlink {
+#endif
+
+#ifndef MAVLINK_HELPER
+#define MAVLINK_HELPER
+#endif
+
+typedef struct {
+ unsigned int sz[2];
+ uint32_t counter[8];
+ union {
+ unsigned char save_bytes[64];
+ uint32_t save_u32[16];
+ } u;
+} mavlink_sha256_ctx;
+
+#define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z)))
+#define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z)))
+
+#define ROTR(x,n) (((x)>>(n)) | ((x) << (32 - (n))))
+
+#define Sigma0(x) (ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22))
+#define Sigma1(x) (ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25))
+#define sigma0(x) (ROTR(x,7) ^ ROTR(x,18) ^ ((x)>>3))
+#define sigma1(x) (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10))
+
+#define A m->counter[0]
+#define B m->counter[1]
+#define C m->counter[2]
+#define D m->counter[3]
+#define E m->counter[4]
+#define F m->counter[5]
+#define G m->counter[6]
+#define H m->counter[7]
+
+static const uint32_t mavlink_sha256_constant_256[64] = {
+ 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
+ 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
+ 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
+ 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
+ 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
+ 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
+ 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
+ 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
+ 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
+ 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
+ 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
+ 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
+ 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
+ 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
+ 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
+ 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
+};
+
+MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m)
+{
+ m->sz[0] = 0;
+ m->sz[1] = 0;
+ A = 0x6a09e667;
+ B = 0xbb67ae85;
+ C = 0x3c6ef372;
+ D = 0xa54ff53a;
+ E = 0x510e527f;
+ F = 0x9b05688c;
+ G = 0x1f83d9ab;
+ H = 0x5be0cd19;
+}
+
+static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in)
+{
+ uint32_t AA, BB, CC, DD, EE, FF, GG, HH;
+ uint32_t data[64];
+ int i;
+
+ AA = A;
+ BB = B;
+ CC = C;
+ DD = D;
+ EE = E;
+ FF = F;
+ GG = G;
+ HH = H;
+
+ for (i = 0; i < 16; ++i)
+ data[i] = in[i];
+ for (i = 16; i < 64; ++i)
+ data[i] = sigma1(data[i-2]) + data[i-7] +
+ sigma0(data[i-15]) + data[i - 16];
+
+ for (i = 0; i < 64; i++) {
+ uint32_t T1, T2;
+
+ T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i];
+ T2 = Sigma0(AA) + Maj(AA,BB,CC);
+
+ HH = GG;
+ GG = FF;
+ FF = EE;
+ EE = DD + T1;
+ DD = CC;
+ CC = BB;
+ BB = AA;
+ AA = T1 + T2;
+ }
+
+ A += AA;
+ B += BB;
+ C += CC;
+ D += DD;
+ E += EE;
+ F += FF;
+ G += GG;
+ H += HH;
+}
+
+MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len)
+{
+ const unsigned char *p = (const unsigned char *)v;
+ uint32_t old_sz = m->sz[0];
+ uint32_t offset;
+
+ m->sz[0] += len * 8;
+ if (m->sz[0] < old_sz)
+ ++m->sz[1];
+ offset = (old_sz / 8) % 64;
+ while(len > 0){
+ uint32_t l = 64 - offset;
+ if (len < l) {
+ l = len;
+ }
+ memcpy(m->u.save_bytes + offset, p, l);
+ offset += l;
+ p += l;
+ len -= l;
+ if(offset == 64){
+ int i;
+ uint32_t current[16];
+ const uint32_t *u = m->u.save_u32;
+ for (i = 0; i < 16; i++){
+ const uint8_t *p1 = (const uint8_t *)&u[i];
+ uint8_t *p2 = (uint8_t *)¤t[i];
+ p2[0] = p1[3];
+ p2[1] = p1[2];
+ p2[2] = p1[1];
+ p2[3] = p1[0];
+ }
+ mavlink_sha256_calc(m, current);
+ offset = 0;
+ }
+ }
+}
+
+/*
+ get first 48 bits of final sha256 hash
+ */
+MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6])
+{
+ unsigned char zeros[72];
+ unsigned offset = (m->sz[0] / 8) % 64;
+ unsigned int dstart = (120 - offset - 1) % 64 + 1;
+ uint8_t *p = (uint8_t *)&m->counter[0];
+
+ *zeros = 0x80;
+ memset (zeros + 1, 0, sizeof(zeros) - 1);
+ zeros[dstart+7] = (m->sz[0] >> 0) & 0xff;
+ zeros[dstart+6] = (m->sz[0] >> 8) & 0xff;
+ zeros[dstart+5] = (m->sz[0] >> 16) & 0xff;
+ zeros[dstart+4] = (m->sz[0] >> 24) & 0xff;
+ zeros[dstart+3] = (m->sz[1] >> 0) & 0xff;
+ zeros[dstart+2] = (m->sz[1] >> 8) & 0xff;
+ zeros[dstart+1] = (m->sz[1] >> 16) & 0xff;
+ zeros[dstart+0] = (m->sz[1] >> 24) & 0xff;
+
+ mavlink_sha256_update(m, zeros, dstart + 8);
+
+ // this ordering makes the result consistent with taking the first
+ // 6 bytes of more conventional sha256 functions. It assumes
+ // little-endian ordering of m->counter
+ result[0] = p[3];
+ result[1] = p[2];
+ result[2] = p[1];
+ result[3] = p[0];
+ result[4] = p[7];
+ result[5] = p[6];
+}
+
+// prevent conflicts with users of the header
+#undef A
+#undef B
+#undef C
+#undef D
+#undef E
+#undef F
+#undef G
+#undef H
+#undef Ch
+#undef ROTR
+#undef Sigma0
+#undef Sigma1
+#undef sigma0
+#undef sigma1
+
+#ifdef MAVLINK_USE_CXX_NAMESPACE
+} // namespace mavlink
+#endif
+
+#endif // HAVE_MAVLINK_SHA256
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_types.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_types.h
new file mode 100755
index 0000000000000000000000000000000000000000..7b616200115339f73451c269fdec82e9ee7a27ae
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/mavlink_types.h
@@ -0,0 +1,298 @@
+#pragma once
+
+// Visual Studio versions before 2010 don't have stdint.h, so we just error out.
+#if (defined _MSC_VER) && (_MSC_VER < 1600)
+#error "The C-MAVLink implementation requires Visual Studio 2010 or greater"
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifdef MAVLINK_USE_CXX_NAMESPACE
+namespace mavlink {
+#endif
+
+// Macro to define packed structures
+#ifdef __GNUC__
+ #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed))
+#else
+ #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
+#endif
+
+#ifndef MAVLINK_MAX_PAYLOAD_LEN
+// it is possible to override this, but be careful!
+#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
+#endif
+
+#define MAVLINK_CORE_HEADER_LEN 9 ///< Length of core header (of the comm. layer)
+#define MAVLINK_CORE_HEADER_MAVLINK1_LEN 5 ///< Length of MAVLink1 core header (of the comm. layer)
+#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and stx
+#define MAVLINK_NUM_CHECKSUM_BYTES 2
+#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
+
+#define MAVLINK_SIGNATURE_BLOCK_LEN 13
+
+#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN) ///< Maximum packet length
+
+/**
+ * Old-style 4 byte param union
+ *
+ * This struct is the data format to be used when sending
+ * parameters. The parameter should be copied to the native
+ * type (without type conversion)
+ * and re-instanted on the receiving side using the
+ * native type as well.
+ */
+MAVPACKED(
+typedef struct param_union {
+ union {
+ float param_float;
+ int32_t param_int32;
+ uint32_t param_uint32;
+ int16_t param_int16;
+ uint16_t param_uint16;
+ int8_t param_int8;
+ uint8_t param_uint8;
+ uint8_t bytes[4];
+ };
+ uint8_t type;
+}) mavlink_param_union_t;
+
+
+/**
+ * New-style 8 byte param union
+ * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering.
+ * The mavlink_param_union_double_t will be treated as a little-endian structure.
+ *
+ * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero.
+ * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the
+ * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union.
+ * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above,
+ * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86,
+ * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number,
+ * and the bits pulled out using the shifts/masks.
+*/
+MAVPACKED(
+typedef struct param_union_extended {
+ union {
+ struct {
+ uint8_t is_double:1;
+ uint8_t mavlink_type:7;
+ union {
+ char c;
+ uint8_t uint8;
+ int8_t int8;
+ uint16_t uint16;
+ int16_t int16;
+ uint32_t uint32;
+ int32_t int32;
+ float f;
+ uint8_t align[7];
+ };
+ };
+ uint8_t data[8];
+ };
+}) mavlink_param_union_double_t;
+
+/**
+ * This structure is required to make the mavlink_send_xxx convenience functions
+ * work, as it tells the library what the current system and component ID are.
+ */
+MAVPACKED(
+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
+}) mavlink_system_t;
+
+MAVPACKED(
+typedef struct __mavlink_message {
+ uint16_t checksum; ///< sent at end of packet
+ uint8_t magic; ///< protocol magic marker
+ uint8_t len; ///< Length of payload
+ uint8_t incompat_flags; ///< flags that must be understood
+ uint8_t compat_flags; ///< flags that can be ignored if not understood
+ uint8_t seq; ///< Sequence of packet
+ uint8_t sysid; ///< ID of message sender system/aircraft
+ uint8_t compid; ///< ID of the message sender component
+ uint32_t msgid:24; ///< ID of message in payload
+ uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
+ uint8_t ck[2]; ///< incoming checksum bytes
+ uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
+}) mavlink_message_t;
+
+typedef enum {
+ MAVLINK_TYPE_CHAR = 0,
+ MAVLINK_TYPE_UINT8_T = 1,
+ MAVLINK_TYPE_INT8_T = 2,
+ MAVLINK_TYPE_UINT16_T = 3,
+ MAVLINK_TYPE_INT16_T = 4,
+ MAVLINK_TYPE_UINT32_T = 5,
+ MAVLINK_TYPE_INT32_T = 6,
+ MAVLINK_TYPE_UINT64_T = 7,
+ MAVLINK_TYPE_INT64_T = 8,
+ MAVLINK_TYPE_FLOAT = 9,
+ MAVLINK_TYPE_DOUBLE = 10
+} mavlink_message_type_t;
+
+#define MAVLINK_MAX_FIELDS 64
+
+typedef struct __mavlink_field_info {
+ const char *name; // name of this field
+ const char *print_format; // printing format hint, or NULL
+ mavlink_message_type_t type; // type of this field
+ unsigned int array_length; // if non-zero, field is an array
+ unsigned int wire_offset; // offset of each field in the payload
+ unsigned int structure_offset; // offset in a C structure
+} mavlink_field_info_t;
+
+// note that in this structure the order of fields is the order
+// in the XML file, not necessary the wire order
+typedef struct __mavlink_message_info {
+ uint32_t msgid; // message ID
+ const char *name; // name of the message
+ unsigned num_fields; // how many fields in this message
+ mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information
+} mavlink_message_info_t;
+
+#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0])))
+#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
+
+// checksum is immediately after the payload bytes
+#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
+#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
+
+typedef enum {
+ MAVLINK_COMM_0,
+ MAVLINK_COMM_1,
+ MAVLINK_COMM_2,
+ MAVLINK_COMM_3
+} mavlink_channel_t;
+
+/*
+ * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
+ * of buffers they will use. If more are used, then the result will be
+ * a stack overrun
+ */
+#ifndef MAVLINK_COMM_NUM_BUFFERS
+#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
+# define MAVLINK_COMM_NUM_BUFFERS 16
+#else
+# define MAVLINK_COMM_NUM_BUFFERS 4
+#endif
+#endif
+
+typedef enum {
+ MAVLINK_PARSE_STATE_UNINIT=0,
+ MAVLINK_PARSE_STATE_IDLE,
+ MAVLINK_PARSE_STATE_GOT_STX,
+ MAVLINK_PARSE_STATE_GOT_LENGTH,
+ MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS,
+ MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS,
+ MAVLINK_PARSE_STATE_GOT_SEQ,
+ MAVLINK_PARSE_STATE_GOT_SYSID,
+ MAVLINK_PARSE_STATE_GOT_COMPID,
+ MAVLINK_PARSE_STATE_GOT_MSGID1,
+ MAVLINK_PARSE_STATE_GOT_MSGID2,
+ MAVLINK_PARSE_STATE_GOT_MSGID3,
+ MAVLINK_PARSE_STATE_GOT_PAYLOAD,
+ MAVLINK_PARSE_STATE_GOT_CRC1,
+ MAVLINK_PARSE_STATE_GOT_BAD_CRC1,
+ MAVLINK_PARSE_STATE_SIGNATURE_WAIT
+} mavlink_parse_state_t; ///< The state machine for the comm parser
+
+typedef enum {
+ MAVLINK_FRAMING_INCOMPLETE=0,
+ MAVLINK_FRAMING_OK=1,
+ MAVLINK_FRAMING_BAD_CRC=2,
+ MAVLINK_FRAMING_BAD_SIGNATURE=3
+} mavlink_framing_t;
+
+#define MAVLINK_STATUS_FLAG_IN_MAVLINK1 1 // last incoming packet was MAVLink1
+#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default
+#define MAVLINK_STATUS_FLAG_IN_SIGNED 4 // last incoming packet was signed and validated
+#define MAVLINK_STATUS_FLAG_IN_BADSIG 8 // last incoming packet had a bad signature
+
+#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol
+
+typedef struct __mavlink_status {
+ uint8_t msg_received; ///< Number of received messages
+ uint8_t buffer_overrun; ///< Number of buffer overruns
+ uint8_t parse_error; ///< Number of parse errors
+ mavlink_parse_state_t parse_state; ///< Parsing state machine
+ uint8_t packet_idx; ///< Index in current packet
+ uint8_t current_rx_seq; ///< Sequence number of last packet received
+ uint8_t current_tx_seq; ///< Sequence number of last packet sent
+ uint16_t packet_rx_success_count; ///< Received packets
+ uint16_t packet_rx_drop_count; ///< Number of packet drops
+ uint8_t flags; ///< MAVLINK_STATUS_FLAG_*
+ uint8_t signature_wait; ///< number of signature bytes left to receive
+ struct __mavlink_signing *signing; ///< optional signing state
+ struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps
+} mavlink_status_t;
+
+/*
+ a callback function to allow for accepting unsigned packets
+ */
+typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32_t msgid);
+
+/*
+ flags controlling signing
+ */
+#define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1
+
+/*
+ state of MAVLink signing for this channel
+ */
+typedef struct __mavlink_signing {
+ uint8_t flags; ///< MAVLINK_SIGNING_FLAG_*
+ uint8_t link_id;
+ uint64_t timestamp;
+ uint8_t secret_key[32];
+ mavlink_accept_unsigned_t accept_unsigned_callback;
+} mavlink_signing_t;
+
+/*
+ timestamp state of each logical signing stream. This needs to be the same structure for all
+ connections in order to be secure
+ */
+#ifndef MAVLINK_MAX_SIGNING_STREAMS
+#define MAVLINK_MAX_SIGNING_STREAMS 16
+#endif
+typedef struct __mavlink_signing_streams {
+ uint16_t num_signing_streams;
+ struct __mavlink_signing_stream {
+ uint8_t link_id;
+ uint8_t sysid;
+ uint8_t compid;
+ uint8_t timestamp_bytes[6];
+ } stream[MAVLINK_MAX_SIGNING_STREAMS];
+} mavlink_signing_streams_t;
+
+
+#define MAVLINK_BIG_ENDIAN 0
+#define MAVLINK_LITTLE_ENDIAN 1
+
+#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM 1
+#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT 2
+
+/*
+ entry in table of information about each message type
+ */
+typedef struct __mavlink_msg_entry {
+ uint32_t msgid;
+ uint8_t crc_extra;
+ uint8_t msg_len;
+ uint8_t flags; // MAV_MSG_ENTRY_FLAG_*
+ uint8_t target_system_ofs; // payload offset to target_system, or 0
+ uint8_t target_component_ofs; // payload offset to target_component, or 0
+} mavlink_msg_entry_t;
+
+/*
+ incompat_flags bits
+ */
+#define MAVLINK_IFLAG_SIGNED 0x01
+#define MAVLINK_IFLAG_MASK 0x01 // mask of all understood bits
+
+#ifdef MAVLINK_USE_CXX_NAMESPACE
+} // namespace mavlink
+#endif
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/protocol.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/protocol.h
new file mode 100755
index 0000000000000000000000000000000000000000..8550277b3e43b8eb04aa7f285bd0cd29cc940772
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/protocol.h
@@ -0,0 +1,334 @@
+#pragma once
+
+#include "string.h"
+#include "mavlink_types.h"
+
+/*
+ If you want MAVLink on a system that is native big-endian,
+ you need to define NATIVE_BIG_ENDIAN
+*/
+#ifdef NATIVE_BIG_ENDIAN
+# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
+#else
+# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
+#endif
+
+#ifndef MAVLINK_STACK_BUFFER
+#define MAVLINK_STACK_BUFFER 0
+#endif
+
+#ifndef MAVLINK_AVOID_GCC_STACK_BUG
+# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
+#endif
+
+#ifndef MAVLINK_ASSERT
+#define MAVLINK_ASSERT(x)
+#endif
+
+#ifndef MAVLINK_START_UART_SEND
+#define MAVLINK_START_UART_SEND(chan, length)
+#endif
+
+#ifndef MAVLINK_END_UART_SEND
+#define MAVLINK_END_UART_SEND(chan, length)
+#endif
+
+/* option to provide alternative implementation of mavlink_helpers.h */
+#ifdef MAVLINK_SEPARATE_HELPERS
+
+ #define MAVLINK_HELPER
+
+ /* decls in sync with those in mavlink_helpers.h */
+ #ifndef MAVLINK_GET_CHANNEL_STATUS
+ MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
+ #endif
+ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
+ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
+ uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra);
+ MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
+ uint8_t min_length, uint8_t length, uint8_t crc_extra);
+ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid, const char *packet,
+ uint8_t min_length, uint8_t length, uint8_t crc_extra);
+ #endif
+ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
+ MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
+ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
+ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
+ mavlink_status_t* status,
+ uint8_t c,
+ mavlink_message_t* r_message,
+ mavlink_status_t* r_mavlink_status);
+ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
+ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
+ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index,
+ uint8_t* r_bit_index, uint8_t* buffer);
+ MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid);
+ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
+ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg);
+ #endif
+
+#else
+
+ #define MAVLINK_HELPER static inline
+ #include "mavlink_helpers.h"
+
+#endif // MAVLINK_SEPARATE_HELPERS
+
+
+/**
+ * @brief Get the required buffer size for this message
+ */
+static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
+{
+ if (msg->magic == MAVLINK_STX_MAVLINK1) {
+ return msg->len + MAVLINK_CORE_HEADER_MAVLINK1_LEN+1 + 2;
+ }
+ uint16_t signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
+ return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES + signature_len;
+}
+
+#if MAVLINK_NEED_BYTE_SWAP
+static inline void byte_swap_2(char *dst, const char *src)
+{
+ dst[0] = src[1];
+ dst[1] = src[0];
+}
+static inline void byte_swap_4(char *dst, const char *src)
+{
+ dst[0] = src[3];
+ dst[1] = src[2];
+ dst[2] = src[1];
+ dst[3] = src[0];
+}
+static inline void byte_swap_8(char *dst, const char *src)
+{
+ dst[0] = src[7];
+ dst[1] = src[6];
+ dst[2] = src[5];
+ dst[3] = src[4];
+ dst[4] = src[3];
+ dst[5] = src[2];
+ dst[6] = src[1];
+ dst[7] = src[0];
+}
+#elif !MAVLINK_ALIGNED_FIELDS
+static inline void byte_copy_2(char *dst, const char *src)
+{
+ dst[0] = src[0];
+ dst[1] = src[1];
+}
+static inline void byte_copy_4(char *dst, const char *src)
+{
+ dst[0] = src[0];
+ dst[1] = src[1];
+ dst[2] = src[2];
+ dst[3] = src[3];
+}
+static inline void byte_copy_8(char *dst, const char *src)
+{
+ memcpy(dst, src, 8);
+}
+#endif
+
+#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
+#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b
+#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b
+
+#if MAVLINK_NEED_BYTE_SWAP
+#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
+#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
+#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
+#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
+#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
+#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
+#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
+#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
+#elif !MAVLINK_ALIGNED_FIELDS
+#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
+#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
+#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
+#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
+#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
+#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
+#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
+#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
+#else
+#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
+#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b
+#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
+#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b
+#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
+#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b
+#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b
+#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b
+#endif
+
+/*
+ like memcpy(), but if src is NULL, do a memset to zero
+*/
+static inline void mav_array_memcpy(void *dest, const void *src, size_t n)
+{
+ if (src == NULL) {
+ memset(dest, 0, n);
+ } else {
+ memcpy(dest, src, n);
+ }
+}
+
+/*
+ * Place a char array into a buffer
+ */
+static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
+{
+ mav_array_memcpy(&buf[wire_offset], b, array_length);
+
+}
+
+/*
+ * Place a uint8_t array into a buffer
+ */
+static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
+{
+ mav_array_memcpy(&buf[wire_offset], b, array_length);
+
+}
+
+/*
+ * Place a int8_t array into a buffer
+ */
+static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
+{
+ mav_array_memcpy(&buf[wire_offset], b, array_length);
+
+}
+
+#if MAVLINK_NEED_BYTE_SWAP
+#define _MAV_PUT_ARRAY(TYPE, V) \
+static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
+{ \
+ if (b == NULL) { \
+ memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
+ } else { \
+ uint16_t i; \
+ for (i=0; i<array_length; i++) { \
+ _mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
+ } \
+ } \
+}
+#else
+#define _MAV_PUT_ARRAY(TYPE, V) \
+static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
+{ \
+ mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
+}
+#endif
+
+_MAV_PUT_ARRAY(uint16_t, u16)
+_MAV_PUT_ARRAY(uint32_t, u32)
+_MAV_PUT_ARRAY(uint64_t, u64)
+_MAV_PUT_ARRAY(int16_t, i16)
+_MAV_PUT_ARRAY(int32_t, i32)
+_MAV_PUT_ARRAY(int64_t, i64)
+_MAV_PUT_ARRAY(float, f)
+_MAV_PUT_ARRAY(double, d)
+
+#define _MAV_RETURN_char(msg, wire_offset) (const char)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_int8_t(msg, wire_offset) (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
+
+#if MAVLINK_NEED_BYTE_SWAP
+#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
+static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
+{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
+
+_MAV_MSG_RETURN_TYPE(uint16_t, 2)
+_MAV_MSG_RETURN_TYPE(int16_t, 2)
+_MAV_MSG_RETURN_TYPE(uint32_t, 4)
+_MAV_MSG_RETURN_TYPE(int32_t, 4)
+_MAV_MSG_RETURN_TYPE(uint64_t, 8)
+_MAV_MSG_RETURN_TYPE(int64_t, 8)
+_MAV_MSG_RETURN_TYPE(float, 4)
+_MAV_MSG_RETURN_TYPE(double, 8)
+
+#elif !MAVLINK_ALIGNED_FIELDS
+#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
+static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
+{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
+
+_MAV_MSG_RETURN_TYPE(uint16_t, 2)
+_MAV_MSG_RETURN_TYPE(int16_t, 2)
+_MAV_MSG_RETURN_TYPE(uint32_t, 4)
+_MAV_MSG_RETURN_TYPE(int32_t, 4)
+_MAV_MSG_RETURN_TYPE(uint64_t, 8)
+_MAV_MSG_RETURN_TYPE(int64_t, 8)
+_MAV_MSG_RETURN_TYPE(float, 4)
+_MAV_MSG_RETURN_TYPE(double, 8)
+#else // nicely aligned, no swap
+#define _MAV_MSG_RETURN_TYPE(TYPE) \
+static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
+{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
+
+_MAV_MSG_RETURN_TYPE(uint16_t)
+_MAV_MSG_RETURN_TYPE(int16_t)
+_MAV_MSG_RETURN_TYPE(uint32_t)
+_MAV_MSG_RETURN_TYPE(int32_t)
+_MAV_MSG_RETURN_TYPE(uint64_t)
+_MAV_MSG_RETURN_TYPE(int64_t)
+_MAV_MSG_RETURN_TYPE(float)
+_MAV_MSG_RETURN_TYPE(double)
+#endif // MAVLINK_NEED_BYTE_SWAP
+
+static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value,
+ uint8_t array_length, uint8_t wire_offset)
+{
+ memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
+ return array_length;
+}
+
+static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value,
+ uint8_t array_length, uint8_t wire_offset)
+{
+ memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
+ return array_length;
+}
+
+static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value,
+ uint8_t array_length, uint8_t wire_offset)
+{
+ memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
+ return array_length;
+}
+
+#if MAVLINK_NEED_BYTE_SWAP
+#define _MAV_RETURN_ARRAY(TYPE, V) \
+static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
+ uint8_t array_length, uint8_t wire_offset) \
+{ \
+ uint16_t i; \
+ for (i=0; i<array_length; i++) { \
+ value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
+ } \
+ return array_length*sizeof(value[0]); \
+}
+#else
+#define _MAV_RETURN_ARRAY(TYPE, V) \
+static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
+ uint8_t array_length, uint8_t wire_offset) \
+{ \
+ memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
+ return array_length*sizeof(TYPE); \
+}
+#endif
+
+_MAV_RETURN_ARRAY(uint16_t, u16)
+_MAV_RETURN_ARRAY(uint32_t, u32)
+_MAV_RETURN_ARRAY(uint64_t, u64)
+_MAV_RETURN_ARRAY(int16_t, i16)
+_MAV_RETURN_ARRAY(int32_t, i32)
+_MAV_RETURN_ARRAY(int64_t, i64)
+_MAV_RETURN_ARRAY(float, f)
+_MAV_RETURN_ARRAY(double, d)
+
+
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink.h
new file mode 100755
index 0000000000000000000000000000000000000000..461759ee6dc0a937534e1829ee03dd90211aac97
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink.h
@@ -0,0 +1,34 @@
+/** @file
+ * @brief MAVLink comm protocol built from velox.xml
+ * @see http://mavlink.org
+ */
+#pragma once
+#ifndef MAVLINK_H
+#define MAVLINK_H
+
+#define MAVLINK_PRIMARY_XML_IDX 0
+
+#ifndef MAVLINK_STX
+#define MAVLINK_STX 253
+#endif
+
+#ifndef MAVLINK_ENDIAN
+#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
+#endif
+
+#ifndef MAVLINK_ALIGNED_FIELDS
+#define MAVLINK_ALIGNED_FIELDS 1
+#endif
+
+#ifndef MAVLINK_CRC_EXTRA
+#define MAVLINK_CRC_EXTRA 1
+#endif
+
+#ifndef MAVLINK_COMMAND_24BIT
+#define MAVLINK_COMMAND_24BIT 1
+#endif
+
+#include "version.h"
+#include "velox.h"
+
+#endif // MAVLINK_H
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_carcontrol.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_carcontrol.h
new file mode 100755
index 0000000000000000000000000000000000000000..edd3b73dc2e9bd2cf967147ca359b05732757b9a
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_carcontrol.h
@@ -0,0 +1,263 @@
+#pragma once
+// MESSAGE CARCONTROL PACKING
+
+#define MAVLINK_MSG_ID_CARCONTROL 50
+
+MAVPACKED(
+typedef struct __mavlink_carcontrol_t {
+ uint32_t timestamp; /*< timestamp*/
+ float vehspd; /*< vehicle speed*/
+ float stang; /*< steering angle*/
+}) mavlink_carcontrol_t;
+
+#define MAVLINK_MSG_ID_CARCONTROL_LEN 12
+#define MAVLINK_MSG_ID_CARCONTROL_MIN_LEN 12
+#define MAVLINK_MSG_ID_50_LEN 12
+#define MAVLINK_MSG_ID_50_MIN_LEN 12
+
+#define MAVLINK_MSG_ID_CARCONTROL_CRC 202
+#define MAVLINK_MSG_ID_50_CRC 202
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_CARCONTROL { \
+ 50, \
+ "CARCONTROL", \
+ 3, \
+ { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_carcontrol_t, timestamp) }, \
+ { "vehspd", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_carcontrol_t, vehspd) }, \
+ { "stang", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_carcontrol_t, stang) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_CARCONTROL { \
+ "CARCONTROL", \
+ 3, \
+ { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_carcontrol_t, timestamp) }, \
+ { "vehspd", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_carcontrol_t, vehspd) }, \
+ { "stang", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_carcontrol_t, stang) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a carcontrol 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 timestamp timestamp
+ * @param vehspd vehicle speed
+ * @param stang steering angle
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_carcontrol_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint32_t timestamp, float vehspd, float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CARCONTROL_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_float(buf, 4, vehspd);
+ _mav_put_float(buf, 8, stang);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CARCONTROL_LEN);
+#else
+ mavlink_carcontrol_t packet;
+ packet.timestamp = timestamp;
+ packet.vehspd = vehspd;
+ packet.stang = stang;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CARCONTROL_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CARCONTROL;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+}
+
+/**
+ * @brief Pack a carcontrol message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param timestamp timestamp
+ * @param vehspd vehicle speed
+ * @param stang steering angle
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_carcontrol_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint32_t timestamp,float vehspd,float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CARCONTROL_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_float(buf, 4, vehspd);
+ _mav_put_float(buf, 8, stang);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CARCONTROL_LEN);
+#else
+ mavlink_carcontrol_t packet;
+ packet.timestamp = timestamp;
+ packet.vehspd = vehspd;
+ packet.stang = stang;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CARCONTROL_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CARCONTROL;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+}
+
+/**
+ * @brief Encode a carcontrol struct
+ *
+ * @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 carcontrol C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_carcontrol_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_carcontrol_t* carcontrol)
+{
+ return mavlink_msg_carcontrol_pack(system_id, component_id, msg, carcontrol->timestamp, carcontrol->vehspd, carcontrol->stang);
+}
+
+/**
+ * @brief Encode a carcontrol struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param carcontrol C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_carcontrol_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_carcontrol_t* carcontrol)
+{
+ return mavlink_msg_carcontrol_pack_chan(system_id, component_id, chan, msg, carcontrol->timestamp, carcontrol->vehspd, carcontrol->stang);
+}
+
+/**
+ * @brief Send a carcontrol message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param timestamp timestamp
+ * @param vehspd vehicle speed
+ * @param stang steering angle
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_carcontrol_send(mavlink_channel_t chan, uint32_t timestamp, float vehspd, float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CARCONTROL_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_float(buf, 4, vehspd);
+ _mav_put_float(buf, 8, stang);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CARCONTROL, buf, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+#else
+ mavlink_carcontrol_t packet;
+ packet.timestamp = timestamp;
+ packet.vehspd = vehspd;
+ packet.stang = stang;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CARCONTROL, (const char *)&packet, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+#endif
+}
+
+/**
+ * @brief Send a carcontrol message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_carcontrol_send_struct(mavlink_channel_t chan, const mavlink_carcontrol_t* carcontrol)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_carcontrol_send(chan, carcontrol->timestamp, carcontrol->vehspd, carcontrol->stang);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CARCONTROL, (const char *)carcontrol, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_CARCONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_carcontrol_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t timestamp, float vehspd, float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_float(buf, 4, vehspd);
+ _mav_put_float(buf, 8, stang);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CARCONTROL, buf, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+#else
+ mavlink_carcontrol_t *packet = (mavlink_carcontrol_t *)msgbuf;
+ packet->timestamp = timestamp;
+ packet->vehspd = vehspd;
+ packet->stang = stang;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CARCONTROL, (const char *)packet, MAVLINK_MSG_ID_CARCONTROL_MIN_LEN, MAVLINK_MSG_ID_CARCONTROL_LEN, MAVLINK_MSG_ID_CARCONTROL_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE CARCONTROL UNPACKING
+
+
+/**
+ * @brief Get field timestamp from carcontrol message
+ *
+ * @return timestamp
+ */
+static inline uint32_t mavlink_msg_carcontrol_get_timestamp(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint32_t(msg, 0);
+}
+
+/**
+ * @brief Get field vehspd from carcontrol message
+ *
+ * @return vehicle speed
+ */
+static inline float mavlink_msg_carcontrol_get_vehspd(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 4);
+}
+
+/**
+ * @brief Get field stang from carcontrol message
+ *
+ * @return steering angle
+ */
+static inline float mavlink_msg_carcontrol_get_stang(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 8);
+}
+
+/**
+ * @brief Decode a carcontrol message into a struct
+ *
+ * @param msg The message to decode
+ * @param carcontrol C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_carcontrol_decode(const mavlink_message_t* msg, mavlink_carcontrol_t* carcontrol)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ carcontrol->timestamp = mavlink_msg_carcontrol_get_timestamp(msg);
+ carcontrol->vehspd = mavlink_msg_carcontrol_get_vehspd(msg);
+ carcontrol->stang = mavlink_msg_carcontrol_get_stang(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_CARCONTROL_LEN? msg->len : MAVLINK_MSG_ID_CARCONTROL_LEN;
+ memset(carcontrol, 0, MAVLINK_MSG_ID_CARCONTROL_LEN);
+ memcpy(carcontrol, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_clocksync.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_clocksync.h
new file mode 100755
index 0000000000000000000000000000000000000000..02ed9767642a9b20d4ee37a1454638cb9a4240dd
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_clocksync.h
@@ -0,0 +1,213 @@
+#pragma once
+// MESSAGE CMD_REQUEST_CLOCKSYNC PACKING
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC 102
+
+MAVPACKED(
+typedef struct __mavlink_cmd_request_clocksync_t {
+ uint8_t dummy; /*< message cannot be empty*/
+}) mavlink_cmd_request_clocksync_t;
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN 1
+#define MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN 1
+#define MAVLINK_MSG_ID_102_LEN 1
+#define MAVLINK_MSG_ID_102_MIN_LEN 1
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC 119
+#define MAVLINK_MSG_ID_102_CRC 119
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_CMD_REQUEST_CLOCKSYNC { \
+ 102, \
+ "CMD_REQUEST_CLOCKSYNC", \
+ 1, \
+ { { "dummy", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cmd_request_clocksync_t, dummy) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_CMD_REQUEST_CLOCKSYNC { \
+ "CMD_REQUEST_CLOCKSYNC", \
+ 1, \
+ { { "dummy", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cmd_request_clocksync_t, dummy) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a cmd_request_clocksync 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 dummy message cannot be empty
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_cmd_request_clocksync_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint8_t dummy)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN];
+ _mav_put_uint8_t(buf, 0, dummy);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN);
+#else
+ mavlink_cmd_request_clocksync_t packet;
+ packet.dummy = dummy;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+}
+
+/**
+ * @brief Pack a cmd_request_clocksync message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param dummy message cannot be empty
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_cmd_request_clocksync_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint8_t dummy)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN];
+ _mav_put_uint8_t(buf, 0, dummy);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN);
+#else
+ mavlink_cmd_request_clocksync_t packet;
+ packet.dummy = dummy;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+}
+
+/**
+ * @brief Encode a cmd_request_clocksync struct
+ *
+ * @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_request_clocksync C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_cmd_request_clocksync_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_request_clocksync_t* cmd_request_clocksync)
+{
+ return mavlink_msg_cmd_request_clocksync_pack(system_id, component_id, msg, cmd_request_clocksync->dummy);
+}
+
+/**
+ * @brief Encode a cmd_request_clocksync struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param cmd_request_clocksync C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_cmd_request_clocksync_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_request_clocksync_t* cmd_request_clocksync)
+{
+ return mavlink_msg_cmd_request_clocksync_pack_chan(system_id, component_id, chan, msg, cmd_request_clocksync->dummy);
+}
+
+/**
+ * @brief Send a cmd_request_clocksync message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param dummy message cannot be empty
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_cmd_request_clocksync_send(mavlink_channel_t chan, uint8_t dummy)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN];
+ _mav_put_uint8_t(buf, 0, dummy);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC, buf, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+#else
+ mavlink_cmd_request_clocksync_t packet;
+ packet.dummy = dummy;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC, (const char *)&packet, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+#endif
+}
+
+/**
+ * @brief Send a cmd_request_clocksync message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_cmd_request_clocksync_send_struct(mavlink_channel_t chan, const mavlink_cmd_request_clocksync_t* cmd_request_clocksync)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_cmd_request_clocksync_send(chan, cmd_request_clocksync->dummy);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC, (const char *)cmd_request_clocksync, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_cmd_request_clocksync_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t dummy)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint8_t(buf, 0, dummy);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC, buf, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+#else
+ mavlink_cmd_request_clocksync_t *packet = (mavlink_cmd_request_clocksync_t *)msgbuf;
+ packet->dummy = dummy;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC, (const char *)packet, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE CMD_REQUEST_CLOCKSYNC UNPACKING
+
+
+/**
+ * @brief Get field dummy from cmd_request_clocksync message
+ *
+ * @return message cannot be empty
+ */
+static inline uint8_t mavlink_msg_cmd_request_clocksync_get_dummy(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 0);
+}
+
+/**
+ * @brief Decode a cmd_request_clocksync message into a struct
+ *
+ * @param msg The message to decode
+ * @param cmd_request_clocksync C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_cmd_request_clocksync_decode(const mavlink_message_t* msg, mavlink_cmd_request_clocksync_t* cmd_request_clocksync)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ cmd_request_clocksync->dummy = mavlink_msg_cmd_request_clocksync_get_dummy(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN? msg->len : MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN;
+ memset(cmd_request_clocksync, 0, MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_LEN);
+ memcpy(cmd_request_clocksync, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_msg.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_msg.h
new file mode 100755
index 0000000000000000000000000000000000000000..c032640ae6be6c3f12b3c0656b24719921975299
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_msg.h
@@ -0,0 +1,263 @@
+#pragma once
+// MESSAGE CMD_REQUEST_MSG PACKING
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_MSG 100
+
+MAVPACKED(
+typedef struct __mavlink_cmd_request_msg_t {
+ uint8_t msgid; /*< message ID of the requested message*/
+ uint8_t active; /*< active if TRUE*/
+ uint8_t period; /*< send every x * 10 ms*/
+}) mavlink_cmd_request_msg_t;
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN 3
+#define MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN 3
+#define MAVLINK_MSG_ID_100_LEN 3
+#define MAVLINK_MSG_ID_100_MIN_LEN 3
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC 216
+#define MAVLINK_MSG_ID_100_CRC 216
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_CMD_REQUEST_MSG { \
+ 100, \
+ "CMD_REQUEST_MSG", \
+ 3, \
+ { { "msgid", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cmd_request_msg_t, msgid) }, \
+ { "active", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_cmd_request_msg_t, active) }, \
+ { "period", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_cmd_request_msg_t, period) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_CMD_REQUEST_MSG { \
+ "CMD_REQUEST_MSG", \
+ 3, \
+ { { "msgid", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cmd_request_msg_t, msgid) }, \
+ { "active", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_cmd_request_msg_t, active) }, \
+ { "period", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_cmd_request_msg_t, period) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a cmd_request_msg 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 msgid message ID of the requested message
+ * @param active active if TRUE
+ * @param period send every x * 10 ms
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_cmd_request_msg_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint8_t msgid, uint8_t active, uint8_t period)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN];
+ _mav_put_uint8_t(buf, 0, msgid);
+ _mav_put_uint8_t(buf, 1, active);
+ _mav_put_uint8_t(buf, 2, period);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN);
+#else
+ mavlink_cmd_request_msg_t packet;
+ packet.msgid = msgid;
+ packet.active = active;
+ packet.period = period;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CMD_REQUEST_MSG;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+}
+
+/**
+ * @brief Pack a cmd_request_msg message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param msgid message ID of the requested message
+ * @param active active if TRUE
+ * @param period send every x * 10 ms
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_cmd_request_msg_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint8_t msgid,uint8_t active,uint8_t period)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN];
+ _mav_put_uint8_t(buf, 0, msgid);
+ _mav_put_uint8_t(buf, 1, active);
+ _mav_put_uint8_t(buf, 2, period);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN);
+#else
+ mavlink_cmd_request_msg_t packet;
+ packet.msgid = msgid;
+ packet.active = active;
+ packet.period = period;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CMD_REQUEST_MSG;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+}
+
+/**
+ * @brief Encode a cmd_request_msg struct
+ *
+ * @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_request_msg C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_cmd_request_msg_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_request_msg_t* cmd_request_msg)
+{
+ return mavlink_msg_cmd_request_msg_pack(system_id, component_id, msg, cmd_request_msg->msgid, cmd_request_msg->active, cmd_request_msg->period);
+}
+
+/**
+ * @brief Encode a cmd_request_msg struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param cmd_request_msg C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_cmd_request_msg_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_request_msg_t* cmd_request_msg)
+{
+ return mavlink_msg_cmd_request_msg_pack_chan(system_id, component_id, chan, msg, cmd_request_msg->msgid, cmd_request_msg->active, cmd_request_msg->period);
+}
+
+/**
+ * @brief Send a cmd_request_msg message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param msgid message ID of the requested message
+ * @param active active if TRUE
+ * @param period send every x * 10 ms
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_cmd_request_msg_send(mavlink_channel_t chan, uint8_t msgid, uint8_t active, uint8_t period)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN];
+ _mav_put_uint8_t(buf, 0, msgid);
+ _mav_put_uint8_t(buf, 1, active);
+ _mav_put_uint8_t(buf, 2, period);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_MSG, buf, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+#else
+ mavlink_cmd_request_msg_t packet;
+ packet.msgid = msgid;
+ packet.active = active;
+ packet.period = period;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_MSG, (const char *)&packet, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+#endif
+}
+
+/**
+ * @brief Send a cmd_request_msg message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_cmd_request_msg_send_struct(mavlink_channel_t chan, const mavlink_cmd_request_msg_t* cmd_request_msg)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_cmd_request_msg_send(chan, cmd_request_msg->msgid, cmd_request_msg->active, cmd_request_msg->period);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_MSG, (const char *)cmd_request_msg, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_cmd_request_msg_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t msgid, uint8_t active, uint8_t period)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint8_t(buf, 0, msgid);
+ _mav_put_uint8_t(buf, 1, active);
+ _mav_put_uint8_t(buf, 2, period);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_MSG, buf, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+#else
+ mavlink_cmd_request_msg_t *packet = (mavlink_cmd_request_msg_t *)msgbuf;
+ packet->msgid = msgid;
+ packet->active = active;
+ packet->period = period;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_MSG, (const char *)packet, MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN, MAVLINK_MSG_ID_CMD_REQUEST_MSG_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE CMD_REQUEST_MSG UNPACKING
+
+
+/**
+ * @brief Get field msgid from cmd_request_msg message
+ *
+ * @return message ID of the requested message
+ */
+static inline uint8_t mavlink_msg_cmd_request_msg_get_msgid(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 0);
+}
+
+/**
+ * @brief Get field active from cmd_request_msg message
+ *
+ * @return active if TRUE
+ */
+static inline uint8_t mavlink_msg_cmd_request_msg_get_active(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 1);
+}
+
+/**
+ * @brief Get field period from cmd_request_msg message
+ *
+ * @return send every x * 10 ms
+ */
+static inline uint8_t mavlink_msg_cmd_request_msg_get_period(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 2);
+}
+
+/**
+ * @brief Decode a cmd_request_msg message into a struct
+ *
+ * @param msg The message to decode
+ * @param cmd_request_msg C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_cmd_request_msg_decode(const mavlink_message_t* msg, mavlink_cmd_request_msg_t* cmd_request_msg)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ cmd_request_msg->msgid = mavlink_msg_cmd_request_msg_get_msgid(msg);
+ cmd_request_msg->active = mavlink_msg_cmd_request_msg_get_active(msg);
+ cmd_request_msg->period = mavlink_msg_cmd_request_msg_get_period(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN? msg->len : MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN;
+ memset(cmd_request_msg, 0, MAVLINK_MSG_ID_CMD_REQUEST_MSG_LEN);
+ memcpy(cmd_request_msg, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_statechange.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_statechange.h
new file mode 100755
index 0000000000000000000000000000000000000000..1982dcdb508d2712f70819fd53180ec80a9d3e1d
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_cmd_request_statechange.h
@@ -0,0 +1,213 @@
+#pragma once
+// MESSAGE CMD_REQUEST_STATECHANGE PACKING
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE 101
+
+MAVPACKED(
+typedef struct __mavlink_cmd_request_statechange_t {
+ uint8_t state; /*< reqeusted state*/
+}) mavlink_cmd_request_statechange_t;
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN 1
+#define MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN 1
+#define MAVLINK_MSG_ID_101_LEN 1
+#define MAVLINK_MSG_ID_101_MIN_LEN 1
+
+#define MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC 229
+#define MAVLINK_MSG_ID_101_CRC 229
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_CMD_REQUEST_STATECHANGE { \
+ 101, \
+ "CMD_REQUEST_STATECHANGE", \
+ 1, \
+ { { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cmd_request_statechange_t, state) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_CMD_REQUEST_STATECHANGE { \
+ "CMD_REQUEST_STATECHANGE", \
+ 1, \
+ { { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cmd_request_statechange_t, state) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a cmd_request_statechange 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 state reqeusted state
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_cmd_request_statechange_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN];
+ _mav_put_uint8_t(buf, 0, state);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN);
+#else
+ mavlink_cmd_request_statechange_t packet;
+ packet.state = state;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+}
+
+/**
+ * @brief Pack a cmd_request_statechange message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param state reqeusted state
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_cmd_request_statechange_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN];
+ _mav_put_uint8_t(buf, 0, state);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN);
+#else
+ mavlink_cmd_request_statechange_t packet;
+ packet.state = state;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+}
+
+/**
+ * @brief Encode a cmd_request_statechange struct
+ *
+ * @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_request_statechange C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_cmd_request_statechange_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_request_statechange_t* cmd_request_statechange)
+{
+ return mavlink_msg_cmd_request_statechange_pack(system_id, component_id, msg, cmd_request_statechange->state);
+}
+
+/**
+ * @brief Encode a cmd_request_statechange struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param cmd_request_statechange C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_cmd_request_statechange_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_request_statechange_t* cmd_request_statechange)
+{
+ return mavlink_msg_cmd_request_statechange_pack_chan(system_id, component_id, chan, msg, cmd_request_statechange->state);
+}
+
+/**
+ * @brief Send a cmd_request_statechange message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param state reqeusted state
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_cmd_request_statechange_send(mavlink_channel_t chan, uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN];
+ _mav_put_uint8_t(buf, 0, state);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE, buf, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+#else
+ mavlink_cmd_request_statechange_t packet;
+ packet.state = state;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE, (const char *)&packet, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+#endif
+}
+
+/**
+ * @brief Send a cmd_request_statechange message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_cmd_request_statechange_send_struct(mavlink_channel_t chan, const mavlink_cmd_request_statechange_t* cmd_request_statechange)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_cmd_request_statechange_send(chan, cmd_request_statechange->state);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE, (const char *)cmd_request_statechange, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_cmd_request_statechange_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint8_t(buf, 0, state);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE, buf, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+#else
+ mavlink_cmd_request_statechange_t *packet = (mavlink_cmd_request_statechange_t *)msgbuf;
+ packet->state = state;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE, (const char *)packet, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE CMD_REQUEST_STATECHANGE UNPACKING
+
+
+/**
+ * @brief Get field state from cmd_request_statechange message
+ *
+ * @return reqeusted state
+ */
+static inline uint8_t mavlink_msg_cmd_request_statechange_get_state(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 0);
+}
+
+/**
+ * @brief Decode a cmd_request_statechange message into a struct
+ *
+ * @param msg The message to decode
+ * @param cmd_request_statechange C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_cmd_request_statechange_decode(const mavlink_message_t* msg, mavlink_cmd_request_statechange_t* cmd_request_statechange)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ cmd_request_statechange->state = mavlink_msg_cmd_request_statechange_get_state(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN? msg->len : MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN;
+ memset(cmd_request_statechange, 0, MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_LEN);
+ memcpy(cmd_request_statechange, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_error.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_error.h
new file mode 100755
index 0000000000000000000000000000000000000000..55c400f9cf2b78f688f14f64524fc0c9d4de7fd9
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_error.h
@@ -0,0 +1,213 @@
+#pragma once
+// MESSAGE ERROR PACKING
+
+#define MAVLINK_MSG_ID_ERROR 1
+
+MAVPACKED(
+typedef struct __mavlink_error_t {
+ uint64_t errorcode; /*< errorcode*/
+}) mavlink_error_t;
+
+#define MAVLINK_MSG_ID_ERROR_LEN 8
+#define MAVLINK_MSG_ID_ERROR_MIN_LEN 8
+#define MAVLINK_MSG_ID_1_LEN 8
+#define MAVLINK_MSG_ID_1_MIN_LEN 8
+
+#define MAVLINK_MSG_ID_ERROR_CRC 77
+#define MAVLINK_MSG_ID_1_CRC 77
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_ERROR { \
+ 1, \
+ "ERROR", \
+ 1, \
+ { { "errorcode", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_error_t, errorcode) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_ERROR { \
+ "ERROR", \
+ 1, \
+ { { "errorcode", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_error_t, errorcode) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a error 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 errorcode errorcode
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_error_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint64_t errorcode)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_ERROR_LEN];
+ _mav_put_uint64_t(buf, 0, errorcode);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ERROR_LEN);
+#else
+ mavlink_error_t packet;
+ packet.errorcode = errorcode;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ERROR_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_ERROR;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+}
+
+/**
+ * @brief Pack a error message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param errorcode errorcode
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_error_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint64_t errorcode)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_ERROR_LEN];
+ _mav_put_uint64_t(buf, 0, errorcode);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ERROR_LEN);
+#else
+ mavlink_error_t packet;
+ packet.errorcode = errorcode;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ERROR_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_ERROR;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+}
+
+/**
+ * @brief Encode a error struct
+ *
+ * @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 error C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_error_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_error_t* error)
+{
+ return mavlink_msg_error_pack(system_id, component_id, msg, error->errorcode);
+}
+
+/**
+ * @brief Encode a error struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param error C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_error_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_error_t* error)
+{
+ return mavlink_msg_error_pack_chan(system_id, component_id, chan, msg, error->errorcode);
+}
+
+/**
+ * @brief Send a error message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param errorcode errorcode
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_error_send(mavlink_channel_t chan, uint64_t errorcode)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_ERROR_LEN];
+ _mav_put_uint64_t(buf, 0, errorcode);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ERROR, buf, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+#else
+ mavlink_error_t packet;
+ packet.errorcode = errorcode;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ERROR, (const char *)&packet, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+#endif
+}
+
+/**
+ * @brief Send a error message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_error_send_struct(mavlink_channel_t chan, const mavlink_error_t* error)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_error_send(chan, error->errorcode);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ERROR, (const char *)error, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_ERROR_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_error_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t errorcode)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint64_t(buf, 0, errorcode);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ERROR, buf, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+#else
+ mavlink_error_t *packet = (mavlink_error_t *)msgbuf;
+ packet->errorcode = errorcode;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ERROR, (const char *)packet, MAVLINK_MSG_ID_ERROR_MIN_LEN, MAVLINK_MSG_ID_ERROR_LEN, MAVLINK_MSG_ID_ERROR_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE ERROR UNPACKING
+
+
+/**
+ * @brief Get field errorcode from error message
+ *
+ * @return errorcode
+ */
+static inline uint64_t mavlink_msg_error_get_errorcode(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint64_t(msg, 0);
+}
+
+/**
+ * @brief Decode a error message into a struct
+ *
+ * @param msg The message to decode
+ * @param error C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_error_decode(const mavlink_message_t* msg, mavlink_error_t* error)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ error->errorcode = mavlink_msg_error_get_errorcode(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_ERROR_LEN? msg->len : MAVLINK_MSG_ID_ERROR_LEN;
+ memset(error, 0, MAVLINK_MSG_ID_ERROR_LEN);
+ memcpy(error, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_heartbeat.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_heartbeat.h
new file mode 100755
index 0000000000000000000000000000000000000000..5bdf1e30965e1d9a5b90a10e5cc4e21bc25110d8
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_heartbeat.h
@@ -0,0 +1,235 @@
+#pragma once
+// MESSAGE HEARTBEAT PACKING
+
+#define MAVLINK_MSG_ID_HEARTBEAT 0
+
+MAVPACKED(
+typedef struct __mavlink_heartbeat_t {
+ uint8_t state; /*< system state*/
+ uint8_t mavlink_version; /*< MAVLink version*/
+}) mavlink_heartbeat_t;
+
+#define MAVLINK_MSG_ID_HEARTBEAT_LEN 2
+#define MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN 2
+#define MAVLINK_MSG_ID_0_LEN 2
+#define MAVLINK_MSG_ID_0_MIN_LEN 2
+
+#define MAVLINK_MSG_ID_HEARTBEAT_CRC 85
+#define MAVLINK_MSG_ID_0_CRC 85
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \
+ 0, \
+ "HEARTBEAT", \
+ 2, \
+ { { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_heartbeat_t, state) }, \
+ { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_heartbeat_t, mavlink_version) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \
+ "HEARTBEAT", \
+ 2, \
+ { { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_heartbeat_t, state) }, \
+ { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_heartbeat_t, mavlink_version) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a heartbeat 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 state system state
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN];
+ _mav_put_uint8_t(buf, 0, state);
+ _mav_put_uint8_t(buf, 1, 1);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+#else
+ mavlink_heartbeat_t packet;
+ packet.state = state;
+ packet.mavlink_version = 1;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+}
+
+/**
+ * @brief Pack a heartbeat message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param state system state
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN];
+ _mav_put_uint8_t(buf, 0, state);
+ _mav_put_uint8_t(buf, 1, 1);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+#else
+ mavlink_heartbeat_t packet;
+ packet.state = state;
+ packet.mavlink_version = 1;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+}
+
+/**
+ * @brief Encode a heartbeat struct
+ *
+ * @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 heartbeat C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
+{
+ return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->state);
+}
+
+/**
+ * @brief Encode a heartbeat struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param heartbeat C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
+{
+ return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->state);
+}
+
+/**
+ * @brief Send a heartbeat message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param state system state
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN];
+ _mav_put_uint8_t(buf, 0, state);
+ _mav_put_uint8_t(buf, 1, 1);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#else
+ mavlink_heartbeat_t packet;
+ packet.state = state;
+ packet.mavlink_version = 1;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#endif
+}
+
+/**
+ * @brief Send a heartbeat message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_heartbeat_send_struct(mavlink_channel_t chan, const mavlink_heartbeat_t* heartbeat)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_heartbeat_send(chan, heartbeat->state);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)heartbeat, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t state)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint8_t(buf, 0, state);
+ _mav_put_uint8_t(buf, 1, 1);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#else
+ mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf;
+ packet->state = state;
+ packet->mavlink_version = 1;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE HEARTBEAT UNPACKING
+
+
+/**
+ * @brief Get field state from heartbeat message
+ *
+ * @return system state
+ */
+static inline uint8_t mavlink_msg_heartbeat_get_state(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 0);
+}
+
+/**
+ * @brief Get field mavlink_version from heartbeat message
+ *
+ * @return MAVLink version
+ */
+static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint8_t(msg, 1);
+}
+
+/**
+ * @brief Decode a heartbeat message into a struct
+ *
+ * @param msg The message to decode
+ * @param heartbeat C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ heartbeat->state = mavlink_msg_heartbeat_get_state(msg);
+ heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_HEARTBEAT_LEN? msg->len : MAVLINK_MSG_ID_HEARTBEAT_LEN;
+ memset(heartbeat, 0, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+ memcpy(heartbeat, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_odometry.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_odometry.h
new file mode 100755
index 0000000000000000000000000000000000000000..13623f0414cbd3eeb3b00a9845d4b5c12502bf05
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_odometry.h
@@ -0,0 +1,363 @@
+#pragma once
+// MESSAGE ODOMETRY PACKING
+
+#define MAVLINK_MSG_ID_ODOMETRY 2
+
+MAVPACKED(
+typedef struct __mavlink_odometry_t {
+ uint32_t timestamp_odom; /*< timestamp odometry*/
+ uint32_t timestamp_stang; /*< timestamp steering angle*/
+ float vehspd_odom; /*< vehicle speed*/
+ float xdist_odom; /*< x distance*/
+ float ydist_odom; /*< y distance*/
+ float yawangle_odom; /*< yaw angle*/
+ float stang; /*< current steering angle*/
+}) mavlink_odometry_t;
+
+#define MAVLINK_MSG_ID_ODOMETRY_LEN 28
+#define MAVLINK_MSG_ID_ODOMETRY_MIN_LEN 28
+#define MAVLINK_MSG_ID_2_LEN 28
+#define MAVLINK_MSG_ID_2_MIN_LEN 28
+
+#define MAVLINK_MSG_ID_ODOMETRY_CRC 9
+#define MAVLINK_MSG_ID_2_CRC 9
+
+
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_ODOMETRY { \
+ 2, \
+ "ODOMETRY", \
+ 7, \
+ { { "timestamp_odom", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_odometry_t, timestamp_odom) }, \
+ { "timestamp_stang", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_odometry_t, timestamp_stang) }, \
+ { "vehspd_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, vehspd_odom) }, \
+ { "xdist_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, xdist_odom) }, \
+ { "ydist_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, ydist_odom) }, \
+ { "yawangle_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_odometry_t, yawangle_odom) }, \
+ { "stang", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_odometry_t, stang) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_ODOMETRY { \
+ "ODOMETRY", \
+ 7, \
+ { { "timestamp_odom", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_odometry_t, timestamp_odom) }, \
+ { "timestamp_stang", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_odometry_t, timestamp_stang) }, \
+ { "vehspd_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, vehspd_odom) }, \
+ { "xdist_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, xdist_odom) }, \
+ { "ydist_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, ydist_odom) }, \
+ { "yawangle_odom", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_odometry_t, yawangle_odom) }, \
+ { "stang", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_odometry_t, stang) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a odometry message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param timestamp_odom timestamp odometry
+ * @param timestamp_stang timestamp steering angle
+ * @param vehspd_odom vehicle speed
+ * @param xdist_odom x distance
+ * @param ydist_odom y distance
+ * @param yawangle_odom yaw angle
+ * @param stang current steering angle
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint32_t timestamp_odom, uint32_t timestamp_stang, float vehspd_odom, float xdist_odom, float ydist_odom, float yawangle_odom, float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_ODOMETRY_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp_odom);
+ _mav_put_uint32_t(buf, 4, timestamp_stang);
+ _mav_put_float(buf, 8, vehspd_odom);
+ _mav_put_float(buf, 12, xdist_odom);
+ _mav_put_float(buf, 16, ydist_odom);
+ _mav_put_float(buf, 20, yawangle_odom);
+ _mav_put_float(buf, 24, stang);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN);
+#else
+ mavlink_odometry_t packet;
+ packet.timestamp_odom = timestamp_odom;
+ packet.timestamp_stang = timestamp_stang;
+ packet.vehspd_odom = vehspd_odom;
+ packet.xdist_odom = xdist_odom;
+ packet.ydist_odom = ydist_odom;
+ packet.yawangle_odom = yawangle_odom;
+ packet.stang = stang;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_ODOMETRY;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+}
+
+/**
+ * @brief Pack a odometry message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param timestamp_odom timestamp odometry
+ * @param timestamp_stang timestamp steering angle
+ * @param vehspd_odom vehicle speed
+ * @param xdist_odom x distance
+ * @param ydist_odom y distance
+ * @param yawangle_odom yaw angle
+ * @param stang current steering angle
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint32_t timestamp_odom,uint32_t timestamp_stang,float vehspd_odom,float xdist_odom,float ydist_odom,float yawangle_odom,float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_ODOMETRY_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp_odom);
+ _mav_put_uint32_t(buf, 4, timestamp_stang);
+ _mav_put_float(buf, 8, vehspd_odom);
+ _mav_put_float(buf, 12, xdist_odom);
+ _mav_put_float(buf, 16, ydist_odom);
+ _mav_put_float(buf, 20, yawangle_odom);
+ _mav_put_float(buf, 24, stang);
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN);
+#else
+ mavlink_odometry_t packet;
+ packet.timestamp_odom = timestamp_odom;
+ packet.timestamp_stang = timestamp_stang;
+ packet.vehspd_odom = vehspd_odom;
+ packet.xdist_odom = xdist_odom;
+ packet.ydist_odom = ydist_odom;
+ packet.yawangle_odom = yawangle_odom;
+ packet.stang = stang;
+
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_ODOMETRY;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+}
+
+/**
+ * @brief Encode a odometry struct
+ *
+ * @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 odometry C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_odometry_t* odometry)
+{
+ return mavlink_msg_odometry_pack(system_id, component_id, msg, odometry->timestamp_odom, odometry->timestamp_stang, odometry->vehspd_odom, odometry->xdist_odom, odometry->ydist_odom, odometry->yawangle_odom, odometry->stang);
+}
+
+/**
+ * @brief Encode a odometry struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param odometry C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_odometry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_odometry_t* odometry)
+{
+ return mavlink_msg_odometry_pack_chan(system_id, component_id, chan, msg, odometry->timestamp_odom, odometry->timestamp_stang, odometry->vehspd_odom, odometry->xdist_odom, odometry->ydist_odom, odometry->yawangle_odom, odometry->stang);
+}
+
+/**
+ * @brief Send a odometry message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param timestamp_odom timestamp odometry
+ * @param timestamp_stang timestamp steering angle
+ * @param vehspd_odom vehicle speed
+ * @param xdist_odom x distance
+ * @param ydist_odom y distance
+ * @param yawangle_odom yaw angle
+ * @param stang current steering angle
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint32_t timestamp_odom, uint32_t timestamp_stang, float vehspd_odom, float xdist_odom, float ydist_odom, float yawangle_odom, float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_ODOMETRY_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp_odom);
+ _mav_put_uint32_t(buf, 4, timestamp_stang);
+ _mav_put_float(buf, 8, vehspd_odom);
+ _mav_put_float(buf, 12, xdist_odom);
+ _mav_put_float(buf, 16, ydist_odom);
+ _mav_put_float(buf, 20, yawangle_odom);
+ _mav_put_float(buf, 24, stang);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+#else
+ mavlink_odometry_t packet;
+ packet.timestamp_odom = timestamp_odom;
+ packet.timestamp_stang = timestamp_stang;
+ packet.vehspd_odom = vehspd_odom;
+ packet.xdist_odom = xdist_odom;
+ packet.ydist_odom = ydist_odom;
+ packet.yawangle_odom = yawangle_odom;
+ packet.stang = stang;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)&packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+#endif
+}
+
+/**
+ * @brief Send a odometry message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_odometry_send_struct(mavlink_channel_t chan, const mavlink_odometry_t* odometry)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_odometry_send(chan, odometry->timestamp_odom, odometry->timestamp_stang, odometry->vehspd_odom, odometry->xdist_odom, odometry->ydist_odom, odometry->yawangle_odom, odometry->stang);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)odometry, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_ODOMETRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t timestamp_odom, uint32_t timestamp_stang, float vehspd_odom, float xdist_odom, float ydist_odom, float yawangle_odom, float stang)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint32_t(buf, 0, timestamp_odom);
+ _mav_put_uint32_t(buf, 4, timestamp_stang);
+ _mav_put_float(buf, 8, vehspd_odom);
+ _mav_put_float(buf, 12, xdist_odom);
+ _mav_put_float(buf, 16, ydist_odom);
+ _mav_put_float(buf, 20, yawangle_odom);
+ _mav_put_float(buf, 24, stang);
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+#else
+ mavlink_odometry_t *packet = (mavlink_odometry_t *)msgbuf;
+ packet->timestamp_odom = timestamp_odom;
+ packet->timestamp_stang = timestamp_stang;
+ packet->vehspd_odom = vehspd_odom;
+ packet->xdist_odom = xdist_odom;
+ packet->ydist_odom = ydist_odom;
+ packet->yawangle_odom = yawangle_odom;
+ packet->stang = stang;
+
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE ODOMETRY UNPACKING
+
+
+/**
+ * @brief Get field timestamp_odom from odometry message
+ *
+ * @return timestamp odometry
+ */
+static inline uint32_t mavlink_msg_odometry_get_timestamp_odom(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint32_t(msg, 0);
+}
+
+/**
+ * @brief Get field timestamp_stang from odometry message
+ *
+ * @return timestamp steering angle
+ */
+static inline uint32_t mavlink_msg_odometry_get_timestamp_stang(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint32_t(msg, 4);
+}
+
+/**
+ * @brief Get field vehspd_odom from odometry message
+ *
+ * @return vehicle speed
+ */
+static inline float mavlink_msg_odometry_get_vehspd_odom(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 8);
+}
+
+/**
+ * @brief Get field xdist_odom from odometry message
+ *
+ * @return x distance
+ */
+static inline float mavlink_msg_odometry_get_xdist_odom(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 12);
+}
+
+/**
+ * @brief Get field ydist_odom from odometry message
+ *
+ * @return y distance
+ */
+static inline float mavlink_msg_odometry_get_ydist_odom(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 16);
+}
+
+/**
+ * @brief Get field yawangle_odom from odometry message
+ *
+ * @return yaw angle
+ */
+static inline float mavlink_msg_odometry_get_yawangle_odom(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 20);
+}
+
+/**
+ * @brief Get field stang from odometry message
+ *
+ * @return current steering angle
+ */
+static inline float mavlink_msg_odometry_get_stang(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_float(msg, 24);
+}
+
+/**
+ * @brief Decode a odometry message into a struct
+ *
+ * @param msg The message to decode
+ * @param odometry C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_odometry_decode(const mavlink_message_t* msg, mavlink_odometry_t* odometry)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ odometry->timestamp_odom = mavlink_msg_odometry_get_timestamp_odom(msg);
+ odometry->timestamp_stang = mavlink_msg_odometry_get_timestamp_stang(msg);
+ odometry->vehspd_odom = mavlink_msg_odometry_get_vehspd_odom(msg);
+ odometry->xdist_odom = mavlink_msg_odometry_get_xdist_odom(msg);
+ odometry->ydist_odom = mavlink_msg_odometry_get_ydist_odom(msg);
+ odometry->yawangle_odom = mavlink_msg_odometry_get_yawangle_odom(msg);
+ odometry->stang = mavlink_msg_odometry_get_stang(msg);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_ODOMETRY_LEN? msg->len : MAVLINK_MSG_ID_ODOMETRY_LEN;
+ memset(odometry, 0, MAVLINK_MSG_ID_ODOMETRY_LEN);
+ memcpy(odometry, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_trajectory.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_trajectory.h
new file mode 100755
index 0000000000000000000000000000000000000000..edff9812fc68f3fa0bbb61e519791eaca1d81df8
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/mavlink_msg_trajectory.h
@@ -0,0 +1,334 @@
+#pragma once
+// MESSAGE TRAJECTORY PACKING
+
+#define MAVLINK_MSG_ID_TRAJECTORY 51
+
+MAVPACKED(
+typedef struct __mavlink_trajectory_t {
+ uint32_t timestamp; /*< timestamp*/
+ int16_t x[10]; /*< Lateral error*/
+ uint16_t y[10]; /*< Longitudinal error*/
+ int16_t theta[10]; /*< course angle error*/
+ int16_t kappa[10]; /*< ground curvature*/
+ int8_t v[10]; /*< nominal speed*/
+}) mavlink_trajectory_t;
+
+#define MAVLINK_MSG_ID_TRAJECTORY_LEN 94
+#define MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN 94
+#define MAVLINK_MSG_ID_51_LEN 94
+#define MAVLINK_MSG_ID_51_MIN_LEN 94
+
+#define MAVLINK_MSG_ID_TRAJECTORY_CRC 4
+#define MAVLINK_MSG_ID_51_CRC 4
+
+#define MAVLINK_MSG_TRAJECTORY_FIELD_X_LEN 10
+#define MAVLINK_MSG_TRAJECTORY_FIELD_Y_LEN 10
+#define MAVLINK_MSG_TRAJECTORY_FIELD_THETA_LEN 10
+#define MAVLINK_MSG_TRAJECTORY_FIELD_KAPPA_LEN 10
+#define MAVLINK_MSG_TRAJECTORY_FIELD_V_LEN 10
+
+#if MAVLINK_COMMAND_24BIT
+#define MAVLINK_MESSAGE_INFO_TRAJECTORY { \
+ 51, \
+ "TRAJECTORY", \
+ 6, \
+ { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_trajectory_t, timestamp) }, \
+ { "x", NULL, MAVLINK_TYPE_INT16_T, 10, 4, offsetof(mavlink_trajectory_t, x) }, \
+ { "y", NULL, MAVLINK_TYPE_UINT16_T, 10, 24, offsetof(mavlink_trajectory_t, y) }, \
+ { "theta", NULL, MAVLINK_TYPE_INT16_T, 10, 44, offsetof(mavlink_trajectory_t, theta) }, \
+ { "kappa", NULL, MAVLINK_TYPE_INT16_T, 10, 64, offsetof(mavlink_trajectory_t, kappa) }, \
+ { "v", NULL, MAVLINK_TYPE_INT8_T, 10, 84, offsetof(mavlink_trajectory_t, v) }, \
+ } \
+}
+#else
+#define MAVLINK_MESSAGE_INFO_TRAJECTORY { \
+ "TRAJECTORY", \
+ 6, \
+ { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_trajectory_t, timestamp) }, \
+ { "x", NULL, MAVLINK_TYPE_INT16_T, 10, 4, offsetof(mavlink_trajectory_t, x) }, \
+ { "y", NULL, MAVLINK_TYPE_UINT16_T, 10, 24, offsetof(mavlink_trajectory_t, y) }, \
+ { "theta", NULL, MAVLINK_TYPE_INT16_T, 10, 44, offsetof(mavlink_trajectory_t, theta) }, \
+ { "kappa", NULL, MAVLINK_TYPE_INT16_T, 10, 64, offsetof(mavlink_trajectory_t, kappa) }, \
+ { "v", NULL, MAVLINK_TYPE_INT8_T, 10, 84, offsetof(mavlink_trajectory_t, v) }, \
+ } \
+}
+#endif
+
+/**
+ * @brief Pack a trajectory 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 timestamp timestamp
+ * @param x Lateral error
+ * @param y Longitudinal error
+ * @param theta course angle error
+ * @param kappa ground curvature
+ * @param v nominal speed
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_trajectory_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+ uint32_t timestamp, const int16_t *x, const uint16_t *y, const int16_t *theta, const int16_t *kappa, const int8_t *v)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_TRAJECTORY_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_int16_t_array(buf, 4, x, 10);
+ _mav_put_uint16_t_array(buf, 24, y, 10);
+ _mav_put_int16_t_array(buf, 44, theta, 10);
+ _mav_put_int16_t_array(buf, 64, kappa, 10);
+ _mav_put_int8_t_array(buf, 84, v, 10);
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_LEN);
+#else
+ mavlink_trajectory_t packet;
+ packet.timestamp = timestamp;
+ mav_array_memcpy(packet.x, x, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.y, y, sizeof(uint16_t)*10);
+ mav_array_memcpy(packet.theta, theta, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.kappa, kappa, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.v, v, sizeof(int8_t)*10);
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_TRAJECTORY;
+ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+}
+
+/**
+ * @brief Pack a trajectory message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param timestamp timestamp
+ * @param x Lateral error
+ * @param y Longitudinal error
+ * @param theta course angle error
+ * @param kappa ground curvature
+ * @param v nominal speed
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_trajectory_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+ mavlink_message_t* msg,
+ uint32_t timestamp,const int16_t *x,const uint16_t *y,const int16_t *theta,const int16_t *kappa,const int8_t *v)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_TRAJECTORY_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_int16_t_array(buf, 4, x, 10);
+ _mav_put_uint16_t_array(buf, 24, y, 10);
+ _mav_put_int16_t_array(buf, 44, theta, 10);
+ _mav_put_int16_t_array(buf, 64, kappa, 10);
+ _mav_put_int8_t_array(buf, 84, v, 10);
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_LEN);
+#else
+ mavlink_trajectory_t packet;
+ packet.timestamp = timestamp;
+ mav_array_memcpy(packet.x, x, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.y, y, sizeof(uint16_t)*10);
+ mav_array_memcpy(packet.theta, theta, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.kappa, kappa, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.v, v, sizeof(int8_t)*10);
+ memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_LEN);
+#endif
+
+ msg->msgid = MAVLINK_MSG_ID_TRAJECTORY;
+ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+}
+
+/**
+ * @brief Encode a trajectory struct
+ *
+ * @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 trajectory C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_trajectory_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_trajectory_t* trajectory)
+{
+ return mavlink_msg_trajectory_pack(system_id, component_id, msg, trajectory->timestamp, trajectory->x, trajectory->y, trajectory->theta, trajectory->kappa, trajectory->v);
+}
+
+/**
+ * @brief Encode a trajectory struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param trajectory C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_trajectory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_trajectory_t* trajectory)
+{
+ return mavlink_msg_trajectory_pack_chan(system_id, component_id, chan, msg, trajectory->timestamp, trajectory->x, trajectory->y, trajectory->theta, trajectory->kappa, trajectory->v);
+}
+
+/**
+ * @brief Send a trajectory message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param timestamp timestamp
+ * @param x Lateral error
+ * @param y Longitudinal error
+ * @param theta course angle error
+ * @param kappa ground curvature
+ * @param v nominal speed
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_trajectory_send(mavlink_channel_t chan, uint32_t timestamp, const int16_t *x, const uint16_t *y, const int16_t *theta, const int16_t *kappa, const int8_t *v)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char buf[MAVLINK_MSG_ID_TRAJECTORY_LEN];
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_int16_t_array(buf, 4, x, 10);
+ _mav_put_uint16_t_array(buf, 24, y, 10);
+ _mav_put_int16_t_array(buf, 44, theta, 10);
+ _mav_put_int16_t_array(buf, 64, kappa, 10);
+ _mav_put_int8_t_array(buf, 84, v, 10);
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY, buf, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+#else
+ mavlink_trajectory_t packet;
+ packet.timestamp = timestamp;
+ mav_array_memcpy(packet.x, x, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.y, y, sizeof(uint16_t)*10);
+ mav_array_memcpy(packet.theta, theta, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.kappa, kappa, sizeof(int16_t)*10);
+ mav_array_memcpy(packet.v, v, sizeof(int8_t)*10);
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+#endif
+}
+
+/**
+ * @brief Send a trajectory message
+ * @param chan MAVLink channel to send the message
+ * @param struct The MAVLink struct to serialize
+ */
+static inline void mavlink_msg_trajectory_send_struct(mavlink_channel_t chan, const mavlink_trajectory_t* trajectory)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ mavlink_msg_trajectory_send(chan, trajectory->timestamp, trajectory->x, trajectory->y, trajectory->theta, trajectory->kappa, trajectory->v);
+#else
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY, (const char *)trajectory, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+#endif
+}
+
+#if MAVLINK_MSG_ID_TRAJECTORY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+ This varient of _send() can be used to save stack space by re-using
+ memory from the receive buffer. The caller provides a
+ mavlink_message_t which is the size of a full mavlink message. This
+ is usually the receive buffer for the channel, and allows a reply to an
+ incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_trajectory_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t timestamp, const int16_t *x, const uint16_t *y, const int16_t *theta, const int16_t *kappa, const int8_t *v)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ char *buf = (char *)msgbuf;
+ _mav_put_uint32_t(buf, 0, timestamp);
+ _mav_put_int16_t_array(buf, 4, x, 10);
+ _mav_put_uint16_t_array(buf, 24, y, 10);
+ _mav_put_int16_t_array(buf, 44, theta, 10);
+ _mav_put_int16_t_array(buf, 64, kappa, 10);
+ _mav_put_int8_t_array(buf, 84, v, 10);
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY, buf, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+#else
+ mavlink_trajectory_t *packet = (mavlink_trajectory_t *)msgbuf;
+ packet->timestamp = timestamp;
+ mav_array_memcpy(packet->x, x, sizeof(int16_t)*10);
+ mav_array_memcpy(packet->y, y, sizeof(uint16_t)*10);
+ mav_array_memcpy(packet->theta, theta, sizeof(int16_t)*10);
+ mav_array_memcpy(packet->kappa, kappa, sizeof(int16_t)*10);
+ mav_array_memcpy(packet->v, v, sizeof(int8_t)*10);
+ _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_LEN, MAVLINK_MSG_ID_TRAJECTORY_CRC);
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE TRAJECTORY UNPACKING
+
+
+/**
+ * @brief Get field timestamp from trajectory message
+ *
+ * @return timestamp
+ */
+static inline uint32_t mavlink_msg_trajectory_get_timestamp(const mavlink_message_t* msg)
+{
+ return _MAV_RETURN_uint32_t(msg, 0);
+}
+
+/**
+ * @brief Get field x from trajectory message
+ *
+ * @return Lateral error
+ */
+static inline uint16_t mavlink_msg_trajectory_get_x(const mavlink_message_t* msg, int16_t *x)
+{
+ return _MAV_RETURN_int16_t_array(msg, x, 10, 4);
+}
+
+/**
+ * @brief Get field y from trajectory message
+ *
+ * @return Longitudinal error
+ */
+static inline uint16_t mavlink_msg_trajectory_get_y(const mavlink_message_t* msg, uint16_t *y)
+{
+ return _MAV_RETURN_uint16_t_array(msg, y, 10, 24);
+}
+
+/**
+ * @brief Get field theta from trajectory message
+ *
+ * @return course angle error
+ */
+static inline uint16_t mavlink_msg_trajectory_get_theta(const mavlink_message_t* msg, int16_t *theta)
+{
+ return _MAV_RETURN_int16_t_array(msg, theta, 10, 44);
+}
+
+/**
+ * @brief Get field kappa from trajectory message
+ *
+ * @return ground curvature
+ */
+static inline uint16_t mavlink_msg_trajectory_get_kappa(const mavlink_message_t* msg, int16_t *kappa)
+{
+ return _MAV_RETURN_int16_t_array(msg, kappa, 10, 64);
+}
+
+/**
+ * @brief Get field v from trajectory message
+ *
+ * @return nominal speed
+ */
+static inline uint16_t mavlink_msg_trajectory_get_v(const mavlink_message_t* msg, int8_t *v)
+{
+ return _MAV_RETURN_int8_t_array(msg, v, 10, 84);
+}
+
+/**
+ * @brief Decode a trajectory message into a struct
+ *
+ * @param msg The message to decode
+ * @param trajectory C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_trajectory_decode(const mavlink_message_t* msg, mavlink_trajectory_t* trajectory)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+ trajectory->timestamp = mavlink_msg_trajectory_get_timestamp(msg);
+ mavlink_msg_trajectory_get_x(msg, trajectory->x);
+ mavlink_msg_trajectory_get_y(msg, trajectory->y);
+ mavlink_msg_trajectory_get_theta(msg, trajectory->theta);
+ mavlink_msg_trajectory_get_kappa(msg, trajectory->kappa);
+ mavlink_msg_trajectory_get_v(msg, trajectory->v);
+#else
+ uint8_t len = msg->len < MAVLINK_MSG_ID_TRAJECTORY_LEN? msg->len : MAVLINK_MSG_ID_TRAJECTORY_LEN;
+ memset(trajectory, 0, MAVLINK_MSG_ID_TRAJECTORY_LEN);
+ memcpy(trajectory, _MAV_PAYLOAD(msg), len);
+#endif
+}
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/testsuite.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/testsuite.h
new file mode 100755
index 0000000000000000000000000000000000000000..d4c09203a5753b2f4453ea97f8a08fabd23f8f33
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/testsuite.h
@@ -0,0 +1,491 @@
+/** @file
+ * @brief MAVLink comm protocol testsuite generated from velox.xml
+ * @see http://qgroundcontrol.org/mavlink/
+ */
+#pragma once
+#ifndef VELOX_TESTSUITE_H
+#define VELOX_TESTSUITE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef MAVLINK_TEST_ALL
+#define MAVLINK_TEST_ALL
+
+static void mavlink_test_velox(uint8_t, uint8_t, mavlink_message_t *last_msg);
+
+static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+
+ mavlink_test_velox(system_id, component_id, last_msg);
+}
+#endif
+
+
+
+
+static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HEARTBEAT >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_heartbeat_t packet_in = {
+ 5,1
+ };
+ mavlink_heartbeat_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.state = packet_in.state;
+ packet1.mavlink_version = packet_in.mavlink_version;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_heartbeat_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.state );
+ mavlink_msg_heartbeat_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.state );
+ mavlink_msg_heartbeat_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_heartbeat_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_heartbeat_send(MAVLINK_COMM_1 , packet1.state );
+ mavlink_msg_heartbeat_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_error(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ERROR >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_error_t packet_in = {
+ 93372036854775807ULL
+ };
+ mavlink_error_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.errorcode = packet_in.errorcode;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_ERROR_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ERROR_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_error_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_error_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_error_pack(system_id, component_id, &msg , packet1.errorcode );
+ mavlink_msg_error_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_error_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.errorcode );
+ mavlink_msg_error_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_error_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_error_send(MAVLINK_COMM_1 , packet1.errorcode );
+ mavlink_msg_error_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_odometry(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ODOMETRY >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_odometry_t packet_in = {
+ 963497464,963497672,73.0,101.0,129.0,157.0,185.0
+ };
+ mavlink_odometry_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.timestamp_odom = packet_in.timestamp_odom;
+ packet1.timestamp_stang = packet_in.timestamp_stang;
+ packet1.vehspd_odom = packet_in.vehspd_odom;
+ packet1.xdist_odom = packet_in.xdist_odom;
+ packet1.ydist_odom = packet_in.ydist_odom;
+ packet1.yawangle_odom = packet_in.yawangle_odom;
+ packet1.stang = packet_in.stang;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_ODOMETRY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ODOMETRY_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_odometry_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_odometry_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_odometry_pack(system_id, component_id, &msg , packet1.timestamp_odom , packet1.timestamp_stang , packet1.vehspd_odom , packet1.xdist_odom , packet1.ydist_odom , packet1.yawangle_odom , packet1.stang );
+ mavlink_msg_odometry_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_odometry_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp_odom , packet1.timestamp_stang , packet1.vehspd_odom , packet1.xdist_odom , packet1.ydist_odom , packet1.yawangle_odom , packet1.stang );
+ mavlink_msg_odometry_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_odometry_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_odometry_send(MAVLINK_COMM_1 , packet1.timestamp_odom , packet1.timestamp_stang , packet1.vehspd_odom , packet1.xdist_odom , packet1.ydist_odom , packet1.yawangle_odom , packet1.stang );
+ mavlink_msg_odometry_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_carcontrol(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CARCONTROL >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_carcontrol_t packet_in = {
+ 963497464,45.0,73.0
+ };
+ mavlink_carcontrol_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.timestamp = packet_in.timestamp;
+ packet1.vehspd = packet_in.vehspd;
+ packet1.stang = packet_in.stang;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_CARCONTROL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CARCONTROL_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_carcontrol_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_carcontrol_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_carcontrol_pack(system_id, component_id, &msg , packet1.timestamp , packet1.vehspd , packet1.stang );
+ mavlink_msg_carcontrol_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_carcontrol_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.vehspd , packet1.stang );
+ mavlink_msg_carcontrol_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_carcontrol_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_carcontrol_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.vehspd , packet1.stang );
+ mavlink_msg_carcontrol_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_trajectory(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TRAJECTORY >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_trajectory_t packet_in = {
+ 963497464,{ 17443, 17444, 17445, 17446, 17447, 17448, 17449, 17450, 17451, 17452 },{ 18483, 18484, 18485, 18486, 18487, 18488, 18489, 18490, 18491, 18492 },{ 19523, 19524, 19525, 19526, 19527, 19528, 19529, 19530, 19531, 19532 },{ 20563, 20564, 20565, 20566, 20567, 20568, 20569, 20570, 20571, 20572 },{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }
+ };
+ mavlink_trajectory_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.timestamp = packet_in.timestamp;
+
+ mav_array_memcpy(packet1.x, packet_in.x, sizeof(int16_t)*10);
+ mav_array_memcpy(packet1.y, packet_in.y, sizeof(uint16_t)*10);
+ mav_array_memcpy(packet1.theta, packet_in.theta, sizeof(int16_t)*10);
+ mav_array_memcpy(packet1.kappa, packet_in.kappa, sizeof(int16_t)*10);
+ mav_array_memcpy(packet1.v, packet_in.v, sizeof(int8_t)*10);
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TRAJECTORY_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_trajectory_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_trajectory_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_trajectory_pack(system_id, component_id, &msg , packet1.timestamp , packet1.x , packet1.y , packet1.theta , packet1.kappa , packet1.v );
+ mavlink_msg_trajectory_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_trajectory_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.x , packet1.y , packet1.theta , packet1.kappa , packet1.v );
+ mavlink_msg_trajectory_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_trajectory_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_trajectory_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.x , packet1.y , packet1.theta , packet1.kappa , packet1.v );
+ mavlink_msg_trajectory_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_cmd_request_msg(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CMD_REQUEST_MSG >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_cmd_request_msg_t packet_in = {
+ 5,72,139
+ };
+ mavlink_cmd_request_msg_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.msgid = packet_in.msgid;
+ packet1.active = packet_in.active;
+ packet1.period = packet_in.period;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CMD_REQUEST_MSG_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_msg_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_cmd_request_msg_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_msg_pack(system_id, component_id, &msg , packet1.msgid , packet1.active , packet1.period );
+ mavlink_msg_cmd_request_msg_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_msg_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.msgid , packet1.active , packet1.period );
+ mavlink_msg_cmd_request_msg_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_cmd_request_msg_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_msg_send(MAVLINK_COMM_1 , packet1.msgid , packet1.active , packet1.period );
+ mavlink_msg_cmd_request_msg_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_cmd_request_statechange(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_cmd_request_statechange_t packet_in = {
+ 5
+ };
+ mavlink_cmd_request_statechange_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.state = packet_in.state;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CMD_REQUEST_STATECHANGE_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_statechange_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_cmd_request_statechange_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_statechange_pack(system_id, component_id, &msg , packet1.state );
+ mavlink_msg_cmd_request_statechange_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_statechange_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.state );
+ mavlink_msg_cmd_request_statechange_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_cmd_request_statechange_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_statechange_send(MAVLINK_COMM_1 , packet1.state );
+ mavlink_msg_cmd_request_statechange_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_cmd_request_clocksync(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
+ if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC >= 256) {
+ return;
+ }
+#endif
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ uint16_t i;
+ mavlink_cmd_request_clocksync_t packet_in = {
+ 5
+ };
+ mavlink_cmd_request_clocksync_t packet1, packet2;
+ memset(&packet1, 0, sizeof(packet1));
+ packet1.dummy = packet_in.dummy;
+
+
+#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
+ if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+ // cope with extensions
+ memset(MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CMD_REQUEST_CLOCKSYNC_MIN_LEN);
+ }
+#endif
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_clocksync_encode(system_id, component_id, &msg, &packet1);
+ mavlink_msg_cmd_request_clocksync_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_clocksync_pack(system_id, component_id, &msg , packet1.dummy );
+ mavlink_msg_cmd_request_clocksync_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_clocksync_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.dummy );
+ mavlink_msg_cmd_request_clocksync_decode(&msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_to_send_buffer(buffer, &msg);
+ for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+ comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+ }
+ mavlink_msg_cmd_request_clocksync_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+ memset(&packet2, 0, sizeof(packet2));
+ mavlink_msg_cmd_request_clocksync_send(MAVLINK_COMM_1 , packet1.dummy );
+ mavlink_msg_cmd_request_clocksync_decode(last_msg, &packet2);
+ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_velox(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+ mavlink_test_heartbeat(system_id, component_id, last_msg);
+ mavlink_test_error(system_id, component_id, last_msg);
+ mavlink_test_odometry(system_id, component_id, last_msg);
+ mavlink_test_carcontrol(system_id, component_id, last_msg);
+ mavlink_test_trajectory(system_id, component_id, last_msg);
+ mavlink_test_cmd_request_msg(system_id, component_id, last_msg);
+ mavlink_test_cmd_request_statechange(system_id, component_id, last_msg);
+ mavlink_test_cmd_request_clocksync(system_id, component_id, last_msg);
+}
+
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+#endif // VELOX_TESTSUITE_H
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/velox.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/velox.h
new file mode 100755
index 0000000000000000000000000000000000000000..9de9436ae181fd745bb4fd2ad33253f7562c8362
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/velox.h
@@ -0,0 +1,100 @@
+/** @file
+ * @brief MAVLink comm protocol generated from velox.xml
+ * @see http://mavlink.org
+ */
+#pragma once
+#ifndef MAVLINK_VELOX_H
+#define MAVLINK_VELOX_H
+
+#ifndef MAVLINK_H
+ #error Wrong include order: MAVLINK_VELOX.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
+#endif
+
+#undef MAVLINK_THIS_XML_IDX
+#define MAVLINK_THIS_XML_IDX 0
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// MESSAGE LENGTHS AND CRCS
+
+#ifndef MAVLINK_MESSAGE_LENGTHS
+#define MAVLINK_MESSAGE_LENGTHS {}
+#endif
+
+#ifndef MAVLINK_MESSAGE_CRCS
+#define MAVLINK_MESSAGE_CRCS {{0, 85, 2, 0, 0, 0}, {1, 77, 8, 0, 0, 0}, {2, 9, 28, 0, 0, 0}, {50, 202, 12, 0, 0, 0}, {51, 4, 94, 0, 0, 0}, {100, 216, 3, 0, 0, 0}, {101, 229, 1, 0, 0, 0}, {102, 119, 1, 0, 0, 0}}
+#endif
+
+#include "../protocol.h"
+
+#define MAVLINK_ENABLED_VELOX
+
+// ENUM DEFINITIONS
+
+
+/** @brief */
+#ifndef HAVE_ENUM_MAV_COMPONENT
+#define HAVE_ENUM_MAV_COMPONENT
+typedef enum MAV_COMPONENT
+{
+ MAV_COMP_ID_STM=0, /* | */
+ MAV_COMP_ID_ADAS=1, /* | */
+ MAV_COMPONENT_ENUM_END=2, /* | */
+} MAV_COMPONENT;
+#endif
+
+/** @brief */
+#ifndef HAVE_ENUM_SYSTEM_STATE
+#define HAVE_ENUM_SYSTEM_STATE
+typedef enum SYSTEM_STATE
+{
+ SYSTEM_STATE_INITIALIZING=0, /* | */
+ SYSTEM_STATE_IDLE=1, /* | */
+ SYSTEM_STATE_RUNNING_EXT=2, /* | */
+ SYSTEM_STATE_RUNNING_RC=3, /* | */
+ SYSTEM_STATE_EMERGENCY=4, /* | */
+ SYSTEM_STATE_NO_REQUEST=255, /* workaround value for requestedState variable when no state change is requested | */
+ SYSTEM_STATE_ENUM_END=256, /* | */
+} SYSTEM_STATE;
+#endif
+
+// MAVLINK VERSION
+
+#ifndef MAVLINK_VERSION
+#define MAVLINK_VERSION 1
+#endif
+
+#if (MAVLINK_VERSION == 0)
+#undef MAVLINK_VERSION
+#define MAVLINK_VERSION 1
+#endif
+
+// MESSAGE DEFINITIONS
+#include "./mavlink_msg_heartbeat.h"
+#include "./mavlink_msg_error.h"
+#include "./mavlink_msg_odometry.h"
+#include "./mavlink_msg_carcontrol.h"
+#include "./mavlink_msg_trajectory.h"
+#include "./mavlink_msg_cmd_request_msg.h"
+#include "./mavlink_msg_cmd_request_statechange.h"
+#include "./mavlink_msg_cmd_request_clocksync.h"
+
+// base include
+
+
+#undef MAVLINK_THIS_XML_IDX
+#define MAVLINK_THIS_XML_IDX 0
+
+#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX
+# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_ERROR, MAVLINK_MESSAGE_INFO_ODOMETRY, MAVLINK_MESSAGE_INFO_CARCONTROL, MAVLINK_MESSAGE_INFO_TRAJECTORY, MAVLINK_MESSAGE_INFO_CMD_REQUEST_MSG, MAVLINK_MESSAGE_INFO_CMD_REQUEST_STATECHANGE, MAVLINK_MESSAGE_INFO_CMD_REQUEST_CLOCKSYNC}
+# if MAVLINK_COMMAND_24BIT
+# include "../mavlink_get_info.h"
+# endif
+#endif
+
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+#endif // MAVLINK_VELOX_H
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/version.h b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/version.h
new file mode 100755
index 0000000000000000000000000000000000000000..aeffde8b0397ba9fc92e7fe039a6b011c7374fbf
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/mavlink/velox/version.h
@@ -0,0 +1,14 @@
+/** @file
+ * @brief MAVLink comm protocol built from velox.xml
+ * @see http://mavlink.org
+ */
+#pragma once
+
+#ifndef MAVLINK_VERSION_H
+#define MAVLINK_VERSION_H
+
+#define MAVLINK_BUILD_DATE "Mon Aug 07 2017"
+#define MAVLINK_WIRE_PROTOCOL_VERSION "2.0"
+#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 94
+
+#endif // MAVLINK_VERSION_H
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/serial_port.h b/modules/catkin_ws/src/VeloxProtocolLib/include/serial_port.h
new file mode 100644
index 0000000000000000000000000000000000000000..2384d1e6f2faacc26149a077dd14af6355da07d2
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/serial_port.h
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 MAVlink Development Team. All rights reserved.
+ * Author: Trent Lukaczyk, <aerialhedgehog@gmail.com>
+ * Jaycee Lock, <jaycee.lock@gmail.com>
+ * Lorenz Meier, <lm@inf.ethz.ch>
+ *
+ * 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. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "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
+ * COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file serial_port.h
+ *
+ * @brief Serial interface definition
+ *
+ * Functions for opening, closing, reading and writing via serial ports
+ *
+ * @author Trent Lukaczyk, <aerialhedgehog@gmail.com>
+ * @author Jaycee Lock, <jaycee.lock@gmail.com>
+ * @author Lorenz Meier, <lm@inf.ethz.ch>
+ *
+ */
+
+#ifndef SERIAL_PORT_H_
+#define SERIAL_PORT_H_
+
+// ------------------------------------------------------------------------------
+// Includes
+// ------------------------------------------------------------------------------
+
+#include <cstdlib>
+#include <stdio.h> // Standard input/output definitions
+#include <unistd.h> // UNIX standard function definitions
+#include <fcntl.h> // File control definitions
+#include <termios.h> // POSIX terminal control definitions
+#include <pthread.h> // This uses POSIX Threads
+#include <signal.h>
+
+#include "mavlink//velox/mavlink.h"
+
+
+// ------------------------------------------------------------------------------
+// Defines
+// ------------------------------------------------------------------------------
+
+// The following two non-standard baudrates should have been defined by the system
+// If not, just fallback to number
+#ifndef B460800
+#define B460800 460800
+#endif
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+
+// Status flags
+#define SERIAL_PORT_OPEN 1;
+#define SERIAL_PORT_CLOSED 0;
+#define SERIAL_PORT_ERROR -1;
+
+
+// ------------------------------------------------------------------------------
+// Prototypes
+// ------------------------------------------------------------------------------
+
+//class Serial_Port;
+
+
+
+// ----------------------------------------------------------------------------------
+// Serial Port Manager Class
+// ----------------------------------------------------------------------------------
+/*
+ * Serial Port Class
+ *
+ * This object handles the opening and closing of the offboard computer's
+ * serial port over which we'll communicate. It also has methods to write
+ * a byte stream buffer. MAVlink is not used in this object yet, it's just
+ * a serialization interface. To help with read and write pthreading, it
+ * gaurds any port operation with a pthread mutex.
+ */
+class Serial_Port
+{
+
+public:
+
+ Serial_Port();
+ Serial_Port(const char *uart_name_, int baudrate_);
+ void initialize_defaults();
+ ~Serial_Port();
+
+ bool debug;
+ const char *uart_name;
+ int baudrate;
+ int status;
+
+ int read_message(mavlink_message_t &message);
+ int write_message(const mavlink_message_t &message);
+
+ void open_serial();
+ void close_serial();
+
+ void start();
+ void stop();
+
+ void handle_quit( int sig );
+
+private:
+
+ int fd;
+ mavlink_status_t lastStatus;
+ pthread_mutex_t lock;
+
+ int _open_port(const char* port);
+ bool _setup_port(int baud, int data_bits, int stop_bits, bool parity, bool hardware_control);
+ int _read_port(uint8_t &cp);
+ int _write_port(char *buf, unsigned len);
+
+};
+
+
+
+#endif // SERIAL_PORT_H_
+
+
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/src/Velox.cpp b/modules/catkin_ws/src/VeloxProtocolLib/src/Velox.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7bf147eee52ab70a2797d4d148214e4d6069b1c6
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/src/Velox.cpp
@@ -0,0 +1,5 @@
+//
+// Created by philipp on 20.04.18.
+//
+
+#include "../include/Velox.h"
\ No newline at end of file
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/src/serial_port.cpp b/modules/catkin_ws/src/VeloxProtocolLib/src/serial_port.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e333163cfd1728580d374239bd31b598bd03139
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/src/serial_port.cpp
@@ -0,0 +1,544 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 MAVlink Development Team. All rights reserved.
+ * Author: Trent Lukaczyk, <aerialhedgehog@gmail.com>
+ * Jaycee Lock, <jaycee.lock@gmail.com>
+ * Lorenz Meier, <lm@inf.ethz.ch>
+ *
+ * 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. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "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
+ * COPYRIGHT OWNER OR CONTRIBUTORS 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file serial_port.cpp
+ *
+ * @brief Serial interface functions
+ *
+ * Functions for opening, closing, reading and writing via serial ports
+ *
+ * @author Trent Lukaczyk, <aerialhedgehog@gmail.com>
+ * @author Jaycee Lock, <jaycee.lock@gmail.com>
+ * @author Lorenz Meier, <lm@inf.ethz.ch>
+ *
+ */
+
+
+// ------------------------------------------------------------------------------
+// Includes
+// ------------------------------------------------------------------------------
+
+#include "../include/serial_port.h"
+
+
+// ----------------------------------------------------------------------------------
+// Serial Port Manager Class
+// ----------------------------------------------------------------------------------
+
+// ------------------------------------------------------------------------------
+// Con/De structors
+// ------------------------------------------------------------------------------
+Serial_Port::
+Serial_Port(const char *uart_name_ , int baudrate_)
+{
+ initialize_defaults();
+ uart_name = uart_name_;
+ baudrate = baudrate_;
+}
+
+Serial_Port::
+Serial_Port()
+{
+ initialize_defaults();
+}
+
+Serial_Port::
+~Serial_Port()
+{
+ // destroy mutex
+ pthread_mutex_destroy(&lock);
+}
+
+void
+Serial_Port::
+initialize_defaults()
+{
+ // Initialize attributes
+ debug = false;
+ fd = -1;
+ status = SERIAL_PORT_CLOSED;
+
+ uart_name = (char*)"/dev/ttyUSB0";
+ baudrate = 57600;
+
+ // Start mutex
+ int result = pthread_mutex_init(&lock, NULL);
+ if ( result != 0 )
+ {
+ printf("\n mutex init failed\n");
+ throw 1;
+ }
+}
+
+
+// ------------------------------------------------------------------------------
+// Read from Serial
+// ------------------------------------------------------------------------------
+int
+Serial_Port::
+read_message(mavlink_message_t &message)
+{
+ uint8_t cp;
+ mavlink_status_t status;
+ uint8_t msgReceived = false;
+
+ // --------------------------------------------------------------------------
+ // READ FROM PORT
+ // --------------------------------------------------------------------------
+
+ // this function locks the port during read
+ int result = _read_port(cp);
+
+
+ // --------------------------------------------------------------------------
+ // PARSE MESSAGE
+ // --------------------------------------------------------------------------
+ if (result > 0)
+ {
+ // the parsing
+ msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
+
+ // check for dropped packets
+ if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug )
+ {
+ printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
+ unsigned char v=cp;
+ fprintf(stderr,"%02x ", v);
+ }
+ lastStatus = status;
+ }
+
+ // Couldn't read from port
+ else
+ {
+ fprintf(stderr, "ERROR: Could not read from fd %d\n", fd);
+ }
+
+ // --------------------------------------------------------------------------
+ // DEBUGGING REPORTS
+ // --------------------------------------------------------------------------
+ if(msgReceived && debug)
+ {
+ // Report info
+ printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
+
+ fprintf(stderr,"Received serial data: ");
+ unsigned int i;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+
+ // check message is write length
+ unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);
+
+ // message length error
+ if (messageLength > MAVLINK_MAX_PACKET_LEN)
+ {
+ fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
+ }
+
+ // print out the buffer
+ else
+ {
+ for (i=0; i<messageLength; i++)
+ {
+ unsigned char v=buffer[i];
+ fprintf(stderr,"%02x ", v);
+ }
+ fprintf(stderr,"\n");
+ }
+ }
+
+ // Done!
+ return msgReceived;
+}
+
+// ------------------------------------------------------------------------------
+// Write to Serial
+// ------------------------------------------------------------------------------
+int
+Serial_Port::
+write_message(const mavlink_message_t &message)
+{
+ char buf[300];
+
+ // Translate message to buffer
+ unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
+
+ // Write buffer to serial port, locks port while writing
+ int bytesWritten = _write_port(buf,len);
+
+ return bytesWritten;
+}
+
+
+// ------------------------------------------------------------------------------
+// Open Serial Port
+// ------------------------------------------------------------------------------
+/**
+ * throws EXIT_FAILURE if could not open the port
+ */
+void
+Serial_Port::
+open_serial()
+{
+
+ // --------------------------------------------------------------------------
+ // OPEN PORT
+ // --------------------------------------------------------------------------
+ printf("OPEN PORT\n");
+
+ fd = _open_port(uart_name);
+
+ // Check success
+ if (fd == -1)
+ {
+ printf("failure, could not open port.\n");
+ throw EXIT_FAILURE;
+ }
+
+ // --------------------------------------------------------------------------
+ // SETUP PORT
+ // --------------------------------------------------------------------------
+ bool success = _setup_port(baudrate, 8, 1, false, false);
+
+ // --------------------------------------------------------------------------
+ // CHECK STATUS
+ // --------------------------------------------------------------------------
+ if (!success)
+ {
+ printf("failure, could not configure port.\n");
+ throw EXIT_FAILURE;
+ }
+ if (fd <= 0)
+ {
+ printf("Connection attempt to port %s with %d baud, 8N1 failed, exiting.\n", uart_name, baudrate);
+ throw EXIT_FAILURE;
+ }
+
+ // --------------------------------------------------------------------------
+ // CONNECTED!
+ // --------------------------------------------------------------------------
+ printf("Connected to %s with %d baud, 8 data bits, no parity, 1 stop bit (8N1)\n", uart_name, baudrate);
+ lastStatus.packet_rx_drop_count = 0;
+
+ status = true;
+
+ printf("\n");
+
+ return;
+
+}
+
+
+// ------------------------------------------------------------------------------
+// Close Serial Port
+// ------------------------------------------------------------------------------
+void
+Serial_Port::
+close_serial()
+{
+ printf("CLOSE PORT\n");
+
+ int result = close(fd);
+
+ if ( result )
+ {
+ fprintf(stderr,"WARNING: Error on port close (%i)\n", result );
+ }
+
+ status = false;
+
+ printf("\n");
+
+}
+
+
+// ------------------------------------------------------------------------------
+// Convenience Functions
+// ------------------------------------------------------------------------------
+void
+Serial_Port::
+start()
+{
+ open_serial();
+}
+
+void
+Serial_Port::
+stop()
+{
+ close_serial();
+}
+
+
+// ------------------------------------------------------------------------------
+// Quit Handler
+// ------------------------------------------------------------------------------
+void
+Serial_Port::
+handle_quit( int sig )
+{
+ try {
+ stop();
+ }
+ catch (int error) {
+ fprintf(stderr,"Warning, could not stop serial port\n");
+ }
+}
+
+
+// ------------------------------------------------------------------------------
+// Helper Function - Open Serial Port File Descriptor
+// ------------------------------------------------------------------------------
+// Where the actual port opening happens, returns file descriptor 'fd'
+int
+Serial_Port::
+_open_port(const char* port)
+{
+ // Open serial port
+ // O_RDWR - Read and write
+ // O_NOCTTY - Ignore special chars like CTRL-C
+ fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+
+ // Check for Errors
+ if (fd == -1)
+ {
+ /* Could not open the port. */
+ return(-1);
+ }
+
+ // Finalize
+ else
+ {
+ fcntl(fd, F_SETFL, 0);
+ }
+
+ // Done!
+ return fd;
+}
+
+// ------------------------------------------------------------------------------
+// Helper Function - Setup Serial Port
+// ------------------------------------------------------------------------------
+// Sets configuration, flags, and baud rate
+bool
+Serial_Port::
+_setup_port(int baud, int data_bits, int stop_bits, bool parity, bool hardware_control)
+{
+ // Check file descriptor
+ if(!isatty(fd))
+ {
+ fprintf(stderr, "\nERROR: file descriptor %d is NOT a serial port\n", fd);
+ return false;
+ }
+
+ // Read file descritor configuration
+ struct termios config;
+ if(tcgetattr(fd, &config) < 0)
+ {
+ fprintf(stderr, "\nERROR: could not read configuration of fd %d\n", fd);
+ return false;
+ }
+
+ // Input flags - Turn off input processing
+ // convert break to null byte, no CR to NL translation,
+ // no NL to CR translation, don't mark parity errors or breaks
+ // no input parity check, don't strip high bit off,
+ // no XON/XOFF software flow control
+ config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+ INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+ // Output flags - Turn off output processing
+ // no CR to NL translation, no NL to CR-NL translation,
+ // no NL to CR translation, no column 0 CR suppression,
+ // no Ctrl-D suppression, no fill characters, no case mapping,
+ // no local output processing
+ config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+ ONOCR | OFILL | OPOST);
+
+ #ifdef OLCUC
+ config.c_oflag &= ~OLCUC;
+ #endif
+
+ #ifdef ONOEOT
+ config.c_oflag &= ~ONOEOT;
+ #endif
+
+ // No line processing:
+ // echo off, echo newline off, canonical mode off,
+ // extended input processing off, signal chars off
+ config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+ // Turn off character processing
+ // clear current char size mask, no parity checking,
+ // no output processing, force 8 bit input
+ config.c_cflag &= ~(CSIZE | PARENB);
+ config.c_cflag |= CS8;
+
+ // One input byte is enough to return from read()
+ // Inter-character timer off
+ config.c_cc[VMIN] = 1;
+ config.c_cc[VTIME] = 10; // was 0
+
+ // Get the current options for the port
+ ////struct termios options;
+ ////tcgetattr(fd, &options);
+
+ // Apply baudrate
+ switch (baud)
+ {
+ case 1200:
+ if (cfsetispeed(&config, B1200) < 0 || cfsetospeed(&config, B1200) < 0)
+ {
+ fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud);
+ return false;
+ }
+ break;
+ case 1800:
+ cfsetispeed(&config, B1800);
+ cfsetospeed(&config, B1800);
+ break;
+ case 9600:
+ cfsetispeed(&config, B9600);
+ cfsetospeed(&config, B9600);
+ break;
+ case 19200:
+ cfsetispeed(&config, B19200);
+ cfsetospeed(&config, B19200);
+ break;
+ case 38400:
+ if (cfsetispeed(&config, B38400) < 0 || cfsetospeed(&config, B38400) < 0)
+ {
+ fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud);
+ return false;
+ }
+ break;
+ case 57600:
+ if (cfsetispeed(&config, B57600) < 0 || cfsetospeed(&config, B57600) < 0)
+ {
+ fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud);
+ return false;
+ }
+ break;
+ case 115200:
+ if (cfsetispeed(&config, B115200) < 0 || cfsetospeed(&config, B115200) < 0)
+ {
+ fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud);
+ return false;
+ }
+ break;
+
+ // These two non-standard (by the 70'ties ) rates are fully supported on
+ // current Debian and Mac OS versions (tested since 2010).
+ case 460800:
+ if (cfsetispeed(&config, B460800) < 0 || cfsetospeed(&config, B460800) < 0)
+ {
+ fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud);
+ return false;
+ }
+ break;
+ case 921600:
+ if (cfsetispeed(&config, B921600) < 0 || cfsetospeed(&config, B921600) < 0)
+ {
+ fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud);
+ return false;
+ }
+ break;
+ default:
+ fprintf(stderr, "ERROR: Desired baud rate %d could not be set, aborting.\n", baud);
+ return false;
+
+ break;
+ }
+
+ // Finally, apply the configuration
+ if(tcsetattr(fd, TCSAFLUSH, &config) < 0)
+ {
+ fprintf(stderr, "\nERROR: could not set configuration of fd %d\n", fd);
+ return false;
+ }
+
+ // Done!
+ return true;
+}
+
+
+
+// ------------------------------------------------------------------------------
+// Read Port with Lock
+// ------------------------------------------------------------------------------
+int
+Serial_Port::
+_read_port(uint8_t &cp)
+{
+
+ // Lock
+ pthread_mutex_lock(&lock);
+
+ int result = read(fd, &cp, 1);
+
+ // Unlock
+ pthread_mutex_unlock(&lock);
+
+ return result;
+}
+
+
+// ------------------------------------------------------------------------------
+// Write Port with Lock
+// ------------------------------------------------------------------------------
+int
+Serial_Port::
+_write_port(char *buf, unsigned len)
+{
+
+ // Lock
+ pthread_mutex_lock(&lock);
+
+ // Write packet via serial link
+ const int bytesWritten = static_cast<int>(write(fd, buf, len));
+
+ // Wait until all data has been written
+ tcdrain(fd);
+
+ // Unlock
+ pthread_mutex_unlock(&lock);
+
+
+ return bytesWritten;
+}
+
+
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/test/Main.cpp b/modules/catkin_ws/src/VeloxProtocolLib/test/Main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4385a8074ee9c39ec818330f547f6ad9238fa16b
--- /dev/null
+++ b/modules/catkin_ws/src/VeloxProtocolLib/test/Main.cpp
@@ -0,0 +1,11 @@
+//
+// Created by philipp on 20.04.18.
+//
+
+#include <iostream>
+
+int main(int argc, char ** argv)
+{
+
+ return 0;
+}
\ No newline at end of file
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index b5bf068a2f8201148fd3b0dac77f9c308be9c8cb..c8702dd04cba6ce428bd8521b3dc13d72221330a 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -4,96 +4,131 @@
#include "../../include/mainNode/PlatoonController.h"
#include "PlatoonProtocolLib/VehicleFacade.h"
-#include "PC2CarLib/TimedValue.h"
+#include "NetworkingLib/TimedValue.h"
#include <iostream>
namespace car
{
-PlatoonController::PlatoonController(platoonProtocol::VehicleFacade& c2c,
- pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
- : cruiseControllerNotify()
+PlatoonController::PlatoonController(platoonProtocol::VehicleFacade & c2c,
+ pc2car::CommandReceiver::Ptr pc, EgoMotion & egoMotion)
+ : cruiseControllerNotify()
, c2c(c2c)
, pc(pc)
, egoMotion(egoMotion)
- , platoonConfig(c2c.getPlatoonConfig())
- {}
+ , platoonConfig(c2c.getPlatoonConfig())
+{}
-void PlatoonController::updatePcConfig() {
- pc2car::TimedValue<platoonProtocol::PlatoonSpeed> otherPS = pc->getPlatoonSpeed();
- pc2car::TimedValue<platoonProtocol::InnerPlatoonDistance> otherIPD = pc->getInnerPlatoonDistance();
-
- if ( otherPS.getTimestamp() > PS.getTimestamp() ) { PS = otherPS; }
- if ( otherIPD.getTimestamp() > IPD.getTimestamp() ) { IPD = otherIPD; }
+void PlatoonController::updatePcConfig()
+{
+ auto otherPS = pc->getPlatoonSpeed();
+ auto otherIPD = pc->getInnerPlatoonDistance();
+
+ if (otherPS.getTimestamp() > PS.getTimestamp())
+ { PS = otherPS; }
+ if (otherIPD.getTimestamp() > IPD.getTimestamp())
+ { IPD = otherIPD; }
cruiseControllerNotify();
}
-void PlatoonController::updateDesSpeed() {
- pc2car::TimedValue<float> otherDesSpeed = pc->getInnerPlatoonDistance();
- if ( otherDesSpeed.getTimestamp() > desSpeed.getTimestamp() ) { desSpeed = otherDesSpeed; }
-
+void PlatoonController::updateDesSpeed()
+{
+ auto otherDesSpeed = pc->getInnerPlatoonDistance();
+ if (otherDesSpeed.getTimestamp() > desSpeed.getTimestamp())
+ { desSpeed = otherDesSpeed; }
+
cruiseControllerNotify();
}
-void PlatoonController::setupC2C() {
+void PlatoonController::setupC2C()
+{
bool wantsPlatoon = pc->isPlatoonEnabled().get();
-
- if ( c2cAlive ) { return; } // if C2C is already alive ... do nothing
- if (!wantsPlatoon ) { return; } // if we dont want platoon ...
-
+
+ if (c2cAlive)
+ { return; } // if C2C is already alive ... do nothing
+ if (!wantsPlatoon)
+ { return; } // if we dont want platoon ...
+
// get role according to distance
- if ( egoMotion.getDistance() == std::numeric_limits<float>::infinity() ) {
+ if (egoMotion.getDistance() == std::numeric_limits<float>::infinity())
+ {
c2c.reset(platoonProtocol::Vehicle::Role::LEADER);
c2c.setInnerPlatoonDistance(IPD.get());
c2c.setPlatoonSpeed(PS.get());
- } else {
+ }
+ else
+ {
c2c.reset(platoonProtocol::Vehicle::Role::FOLLOWER);
}
c2c.createPlatoon();
c2cAlive = true;
}
-void PlatoonController::run() {
+void PlatoonController::run()
+{
std::cout << "PlatoonController was run." << std::endl;
- switch (curState) {
- case PlatoonState::ACC: { run_ACC(); break; }
- case PlatoonState::CACC_FV: { run_CACC_FV(); break; }
- case PlatoonState::CACC_LV: { run_CACC_LV(); break; }
+ switch (curState)
+ {
+ case PlatoonState::ACC:
+ {
+ run_ACC();
+ break;
+ }
+ case PlatoonState::CACC_FV:
+ {
+ run_CACC_FV();
+ break;
+ }
+ case PlatoonState::CACC_LV:
+ {
+ run_CACC_LV();
+ break;
+ }
}
}
-void PlatoonController::run_ACC() {
-
+void PlatoonController::run_ACC()
+{
bool wantsPlatoon = pc->isPlatoonEnabled().get();
- if (!wantsPlatoon) {
- if (c2cAlive) { c2c.leavePlatoon(); c2cAlive = false;}
+ if (!wantsPlatoon)
+ {
+ if (c2cAlive)
+ {
+ c2c.leavePlatoon();
+ c2cAlive = false;
+ }
updateDesSpeed();
return;
- }
-
+ }
+
// wantsPlatoon
- if (!c2cAlive) {
+ if (!c2cAlive)
+ {
updateDesSpeed();
setupC2C();
return;
}
// c2cAlive + wantsPlatoon
- if (c2c.isPlatoonRunning()) {
- if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {
+ if (c2c.isPlatoonRunning())
+ {
+ if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
+ {
updatePcConfig();
- curState = PlatoonState::CACC_LV;
+ curState = PlatoonState::CACC_LV;
}
- else {curState = PlatoonState::CACC_FV;}
+ else
+ { curState = PlatoonState::CACC_FV; }
return;
}
// !inPlatoon + c2cAlive + wantsPlatoon
bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
bool hasFiniteDistance = egoMotion.getDistance() < std::numeric_limits<float>::infinity();
- if ( isLeader != hasFiniteDistance ) { // role does not fit
+ if (isLeader != hasFiniteDistance)
+ { // role does not fit
c2c.leavePlatoon();
c2cAlive = false;
updateDesSpeed();
@@ -102,26 +137,33 @@ void PlatoonController::run_ACC() {
}
// role fits + !inPlatoon + c2cAlive + wantsPlatoon
- if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {updatePcConfig();}
+ if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
+ { updatePcConfig(); }
updateDesSpeed();
return;
}
-void PlatoonController::run_CACC_FV() {
- bool inPlatoon = c2c.isPlatoonRunning();
+void PlatoonController::run_CACC_FV()
+{
+ bool inPlatoon = c2c.isPlatoonRunning();
bool wantsPlatoon = pc->isPlatoonEnabled().get();
-
+
// Although this value will not be used in this method it still
// needs to be updated, so the new value can be pulled be CC
platoonConfig = c2c.getPlatoonConfig(); // TODO <- updateC2CConfig();
-
+
std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
- << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+ << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+
+ if (inPlatoon && wantsPlatoon)
+ {
+ cruiseControllerNotify();
+ return;
+ }
+
+ if (inPlatoon && !wantsPlatoon)
+ { c2c.leavePlatoon(); }
- if (inPlatoon && wantsPlatoon) { cruiseControllerNotify(); return; }
-
- if (inPlatoon && !wantsPlatoon) { c2c.leavePlatoon(); }
-
c2cAlive = false;
curState = PlatoonState::ACC;
cruiseControllerNotify();
@@ -129,24 +171,27 @@ void PlatoonController::run_CACC_FV() {
return;
}
-void PlatoonController::run_CACC_LV() {
- bool inPlatoon = c2c.isPlatoonRunning();
+void PlatoonController::run_CACC_LV()
+{
+ bool inPlatoon = c2c.isPlatoonRunning();
bool wantsPlatoon = pc->isPlatoonEnabled().get();
-
+
std::cout << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
- << ", wantsPlatoon = " << wantsPlatoon << std::endl;
-
+ << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+
- if (inPlatoon && wantsPlatoon) {
+ if (inPlatoon && wantsPlatoon)
+ {
updatePcConfig();
c2c.setInnerPlatoonDistance(IPD.get());
return;
- }
-
- if (inPlatoon && !wantsPlatoon) {
+ }
+
+ if (inPlatoon && !wantsPlatoon)
+ {
c2c.leavePlatoon();
}
-
+
c2cAlive = false;
curState = PlatoonState::ACC;
cruiseControllerNotify();