diff --git a/unitree_lidar_sdk/CMakeLists.txt b/unitree_lidar_sdk/CMakeLists.txt index b5eea76..fb9cc3d 100755 --- a/unitree_lidar_sdk/CMakeLists.txt +++ b/unitree_lidar_sdk/CMakeLists.txt @@ -3,8 +3,8 @@ project(unitree_lidar_sdk) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++14") -set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb -llz4") -set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -DNDEBUG -llz4") +set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb") +set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -DNDEBUG") include_directories(include) diff --git a/unitree_lidar_sdk/README.md b/unitree_lidar_sdk/README.md index 1794a9a..4caf616 100755 --- a/unitree_lidar_sdk/README.md +++ b/unitree_lidar_sdk/README.md @@ -148,4 +148,9 @@ sudo usermod -a -G dialout $USER ### v1.0.7 (2023.06.21) - Support firmware version: 1.0.1 - Add coordinate system definition in readme -- Modified readme of `unitree_lidar_ros` and `unitree_lidar_ros2` \ No newline at end of file +- Modified readme of `unitree_lidar_ros` and `unitree_lidar_ros2` + +### v1.0.8 (2023.06.28) +- Support firmware version: 1.0.1 +- Solve the dependency problem --- `cannot find -llz4` +- Add mavlink headers for optional use \ No newline at end of file diff --git a/unitree_lidar_sdk/examples/example_lidar.cpp b/unitree_lidar_sdk/examples/example_lidar.cpp index e7d0541..940d232 100644 --- a/unitree_lidar_sdk/examples/example_lidar.cpp +++ b/unitree_lidar_sdk/examples/example_lidar.cpp @@ -74,7 +74,7 @@ int main(){ printf("\n"); sleep(2); - + // Set Lidar Working Mode printf("Set Lidar working mode to: NORMAL_MODE ... \n"); lreader->setLidarWorkingMode(NORMAL); diff --git a/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a b/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a index eb8cee9..79ff312 100644 Binary files a/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a and b/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a differ diff --git a/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a b/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a index 791f22a..8031b93 100644 Binary files a/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a and b/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a differ diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/SysMavlink.h b/unitree_lidar_sdk/mavlink/SysMavlink/SysMavlink.h new file mode 100644 index 0000000..58262bd --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/SysMavlink.h @@ -0,0 +1,136 @@ +/** @file + * @brief MAVLink comm protocol generated from SysMavlink.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_SYSMAVLINK_H +#define MAVLINK_SYSMAVLINK_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_SYSMAVLINK.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_HASH +#define MAVLINK_THIS_XML_HASH -8759955630894977959 + +#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 {{11, 106, 1, 1, 0, 0, 0}, {12, 204, 1, 1, 0, 0, 0}, {13, 62, 38, 38, 0, 0, 0}, {14, 84, 1, 1, 0, 0, 0}, {15, 236, 56, 56, 0, 0, 0}, {16, 74, 246, 246, 0, 0, 0}, {17, 99, 209, 209, 0, 0, 0}, {18, 12, 5, 5, 0, 0, 0}, {19, 110, 42, 42, 0, 0, 0}, {121, 228, 1, 1, 0, 0, 0}, {122, 151, 20, 20, 0, 0, 0}, {123, 10, 61, 61, 0, 0, 0}, {124, 68, 44, 44, 0, 0, 0}, {125, 149, 36, 36, 0, 0, 0}, {126, 25, 1, 1, 0, 0, 0}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_SYSMAVLINK + +// ENUM DEFINITIONS + +/** @brief */ +#ifndef HAVE_ENUM_LIDAR_WORKING_MODE +#define HAVE_ENUM_LIDAR_WORKING_MODE +typedef enum LIDAR_WORKING_MODE +{ + NORMAL_MODE=1, /* Configure the LiDAR to operate in normal mode. | */ + STANDBY_MODE=2, /* Configure the LiDAR to enter standby mode. | */ + LIDAR_WORKING_MODE_ENUM_END=3, /* | */ +} LIDAR_WORKING_MODE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_DEVICE_REQUEST_DATA_TYPE +#define HAVE_ENUM_DEVICE_REQUEST_DATA_TYPE +typedef enum DEVICE_REQUEST_DATA_TYPE +{ + DATA_REQUEST_CLOCK_SYNC=1, /* Request for device state | */ + CMD_LIDAR_VERSION=2, /* Request get firmware version | */ + DEVICE_REQUEST_DATA_TYPE_ENUM_END=3, /* | */ +} DEVICE_REQUEST_DATA_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_DEVICE_COMMAND_TYPE +#define HAVE_ENUM_DEVICE_COMMAND_TYPE +typedef enum DEVICE_COMMAND_TYPE +{ + CMD_LIDAR_SAVE_FLASH=1, /* Save lidar configuration to Flash memory | */ + CMD_LIDAR_REBOOT=2, /* Reboot lidar | */ + DEVICE_COMMAND_TYPE_ENUM_END=3, /* | */ +} DEVICE_COMMAND_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_SYNC_TIME_TYPE +#define HAVE_ENUM_SYNC_TIME_TYPE +typedef enum SYNC_TIME_TYPE +{ + CMD_SYNC_TIME_REQUEST=1, /* Lidar sync time request start | */ + CMD_SYNC_TIME_RESPONSE=2, /* Lidar sync time request response | */ + SYNC_TIME_TYPE_ENUM_END=3, /* | */ +} SYNC_TIME_TYPE; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_LED_DISPLAY_MODE_SELECT +#define HAVE_ENUM_LED_DISPLAY_MODE_SELECT +typedef enum LED_DISPLAY_MODE_SELECT +{ + LED_RING_COMMAND_MODE=1, /* Command display mode. | */ + LED_RING_FUN_FORWARD_SLOW_MODE=2, /* Function mode forward at slow. | */ + LED_RING_FUN_FORWARD_FAST_MODE=3, /* Function mode forward at fast. | */ + LED_RING_FUN_REVERSE_SLOW_MODE=4, /* Function mode reverse at slow. | */ + LED_RING_FUN_REVERSE_FAST_MODE=5, /* Function mode reverse at fast. | */ + LED_RING_FUN_TRIPLE_FLIP_MODE=6, /* Function mode triple flip. | */ + LED_RING_FUN_TRIPLE_BREATHING_MODE=7, /* Function mode triple breathing. | */ + LED_RING_FUN_SIXSTAGE_BREATHING_MODE=8, /* Function mode six-stage breathing. | */ + LED_DISPLAY_MODE_SELECT_ENUM_END=9, /* | */ +} LED_DISPLAY_MODE_SELECT; +#endif + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 33 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 33 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_device_request_data.h" +#include "./mavlink_msg_device_command.h" +#include "./mavlink_msg_ret_lidar_version.h" +#include "./mavlink_msg_config_lidar_working_mode.h" +#include "./mavlink_msg_config_led_ring_table_packet.h" +#include "./mavlink_msg_ret_lidar_distance_data_packet.h" +#include "./mavlink_msg_ret_lidar_auxiliary_data_packet.h" +#include "./mavlink_msg_ret_lidar_time_sync_data.h" +#include "./mavlink_msg_ret_imu_attitude_data_packet.h" + +// base include + + +#undef MAVLINK_THIS_XML_HASH +#define MAVLINK_THIS_XML_HASH -8759955630894977959 + +#if MAVLINK_THIS_XML_HASH == MAVLINK_PRIMARY_XML_HASH +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_DEVICE_REQUEST_DATA, MAVLINK_MESSAGE_INFO_DEVICE_COMMAND, MAVLINK_MESSAGE_INFO_RET_LIDAR_VERSION, MAVLINK_MESSAGE_INFO_CONFIG_LIDAR_WORKING_MODE, MAVLINK_MESSAGE_INFO_CONFIG_LED_RING_TABLE_PACKET, MAVLINK_MESSAGE_INFO_RET_LIDAR_DISTANCE_DATA_PACKET, MAVLINK_MESSAGE_INFO_RET_LIDAR_AUXILIARY_DATA_PACKET, MAVLINK_MESSAGE_INFO_RET_LIDAR_TIME_SYNC_DATA, MAVLINK_MESSAGE_INFO_RET_IMU_ATTITUDE_DATA_PACKET, MAVLINK_MESSAGE_INFO_DEVICE_REQUEST_ANSWER, MAVLINK_MESSAGE_INFO_DEVICE_DEBUG_MODE_PARAMS, MAVLINK_MESSAGE_INFO_SET_COM_NECESSARY_MEASURE_PARAMETER, MAVLINK_MESSAGE_INFO_SET_SYS_NECESSARY_MEASURE_PARAMETER, MAVLINK_MESSAGE_INFO_RET_TDC_POINT_DEBUG_PACKET, MAVLINK_MESSAGE_INFO_DEVICE_REQUEST_INTERNAL_DATA} +# define MAVLINK_MESSAGE_NAMES {{ "CONFIG_LED_RING_TABLE_PACKET", 15 }, { "CONFIG_LIDAR_WORKING_MODE", 14 }, { "DEVICE_COMMAND", 12 }, { "DEVICE_DEBUG_MODE_PARAMS", 122 }, { "DEVICE_REQUEST_ANSWER", 121 }, { "DEVICE_REQUEST_DATA", 11 }, { "DEVICE_REQUEST_INTERNAL_DATA", 126 }, { "RET_IMU_ATTITUDE_DATA_PACKET", 19 }, { "RET_LIDAR_AUXILIARY_DATA_PACKET", 17 }, { "RET_LIDAR_DISTANCE_DATA_PACKET", 16 }, { "RET_LIDAR_TIME_SYNC_DATA", 18 }, { "RET_LIDAR_VERSION", 13 }, { "RET_TDC_POINT_DEBUG_PACKET", 125 }, { "SET_COM_NECESSARY_MEASURE_PARAMETER", 123 }, { "SET_SYS_NECESSARY_MEASURE_PARAMETER", 124 }} +# if MAVLINK_COMMAND_24BIT +# include "../mavlink_get_info.h" +# endif +#endif + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_SYSMAVLINK_H diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink.h new file mode 100644 index 0000000..276c379 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink.h @@ -0,0 +1,34 @@ +/** @file + * @brief MAVLink comm protocol built from SysMavlink.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_H +#define MAVLINK_H + +#define MAVLINK_PRIMARY_XML_HASH -8759955630894977959 + +#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 "SysMavlink.h" + +#endif // MAVLINK_H diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_config_led_ring_table_packet.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_config_led_ring_table_packet.h new file mode 100644 index 0000000..ac162c9 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_config_led_ring_table_packet.h @@ -0,0 +1,305 @@ +#pragma once +// MESSAGE CONFIG_LED_RING_TABLE_PACKET PACKING + +#define MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET 15 + + +typedef struct __mavlink_config_led_ring_table_packet_t { + uint32_t led_rotation_period; /*< Rotation period.*/ + uint32_t led_zero_point_offset; /*< Zero offset unit in milliseconds.*/ + uint16_t packet_id; /*< Data packet ID.*/ + uint8_t led_display_mode; /*< LED display mode selection.*/ + uint8_t led_table[45]; /*< LED display field data packet, 45 bytes represent the status of 360 LEDs bit by bit.*/ +} mavlink_config_led_ring_table_packet_t; + +#define MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN 56 +#define MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN 56 +#define MAVLINK_MSG_ID_15_LEN 56 +#define MAVLINK_MSG_ID_15_MIN_LEN 56 + +#define MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC 236 +#define MAVLINK_MSG_ID_15_CRC 236 + +#define MAVLINK_MSG_CONFIG_LED_RING_TABLE_PACKET_FIELD_LED_TABLE_LEN 45 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CONFIG_LED_RING_TABLE_PACKET { \ + 15, \ + "CONFIG_LED_RING_TABLE_PACKET", \ + 5, \ + { { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_config_led_ring_table_packet_t, packet_id) }, \ + { "led_rotation_period", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_config_led_ring_table_packet_t, led_rotation_period) }, \ + { "led_zero_point_offset", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_config_led_ring_table_packet_t, led_zero_point_offset) }, \ + { "led_display_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_config_led_ring_table_packet_t, led_display_mode) }, \ + { "led_table", NULL, MAVLINK_TYPE_UINT8_T, 45, 11, offsetof(mavlink_config_led_ring_table_packet_t, led_table) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CONFIG_LED_RING_TABLE_PACKET { \ + "CONFIG_LED_RING_TABLE_PACKET", \ + 5, \ + { { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_config_led_ring_table_packet_t, packet_id) }, \ + { "led_rotation_period", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_config_led_ring_table_packet_t, led_rotation_period) }, \ + { "led_zero_point_offset", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_config_led_ring_table_packet_t, led_zero_point_offset) }, \ + { "led_display_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_config_led_ring_table_packet_t, led_display_mode) }, \ + { "led_table", NULL, MAVLINK_TYPE_UINT8_T, 45, 11, offsetof(mavlink_config_led_ring_table_packet_t, led_table) }, \ + } \ +} +#endif + +/** + * @brief Pack a config_led_ring_table_packet 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 packet_id Data packet ID. + * @param led_rotation_period Rotation period. + * @param led_zero_point_offset Zero offset unit in milliseconds. + * @param led_display_mode LED display mode selection. + * @param led_table LED display field data packet, 45 bytes represent the status of 360 LEDs bit by bit. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_config_led_ring_table_packet_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t packet_id, uint32_t led_rotation_period, uint32_t led_zero_point_offset, uint8_t led_display_mode, const uint8_t *led_table) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN]; + _mav_put_uint32_t(buf, 0, led_rotation_period); + _mav_put_uint32_t(buf, 4, led_zero_point_offset); + _mav_put_uint16_t(buf, 8, packet_id); + _mav_put_uint8_t(buf, 10, led_display_mode); + _mav_put_uint8_t_array(buf, 11, led_table, 45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN); +#else + mavlink_config_led_ring_table_packet_t packet; + packet.led_rotation_period = led_rotation_period; + packet.led_zero_point_offset = led_zero_point_offset; + packet.packet_id = packet_id; + packet.led_display_mode = led_display_mode; + mav_array_memcpy(packet.led_table, led_table, sizeof(uint8_t)*45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +} + +/** + * @brief Pack a config_led_ring_table_packet 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 packet_id Data packet ID. + * @param led_rotation_period Rotation period. + * @param led_zero_point_offset Zero offset unit in milliseconds. + * @param led_display_mode LED display mode selection. + * @param led_table LED display field data packet, 45 bytes represent the status of 360 LEDs bit by bit. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_config_led_ring_table_packet_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t packet_id,uint32_t led_rotation_period,uint32_t led_zero_point_offset,uint8_t led_display_mode,const uint8_t *led_table) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN]; + _mav_put_uint32_t(buf, 0, led_rotation_period); + _mav_put_uint32_t(buf, 4, led_zero_point_offset); + _mav_put_uint16_t(buf, 8, packet_id); + _mav_put_uint8_t(buf, 10, led_display_mode); + _mav_put_uint8_t_array(buf, 11, led_table, 45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN); +#else + mavlink_config_led_ring_table_packet_t packet; + packet.led_rotation_period = led_rotation_period; + packet.led_zero_point_offset = led_zero_point_offset; + packet.packet_id = packet_id; + packet.led_display_mode = led_display_mode; + mav_array_memcpy(packet.led_table, led_table, sizeof(uint8_t)*45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +} + +/** + * @brief Encode a config_led_ring_table_packet 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 config_led_ring_table_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_config_led_ring_table_packet_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_config_led_ring_table_packet_t* config_led_ring_table_packet) +{ + return mavlink_msg_config_led_ring_table_packet_pack(system_id, component_id, msg, config_led_ring_table_packet->packet_id, config_led_ring_table_packet->led_rotation_period, config_led_ring_table_packet->led_zero_point_offset, config_led_ring_table_packet->led_display_mode, config_led_ring_table_packet->led_table); +} + +/** + * @brief Encode a config_led_ring_table_packet 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 config_led_ring_table_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_config_led_ring_table_packet_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_config_led_ring_table_packet_t* config_led_ring_table_packet) +{ + return mavlink_msg_config_led_ring_table_packet_pack_chan(system_id, component_id, chan, msg, config_led_ring_table_packet->packet_id, config_led_ring_table_packet->led_rotation_period, config_led_ring_table_packet->led_zero_point_offset, config_led_ring_table_packet->led_display_mode, config_led_ring_table_packet->led_table); +} + +/** + * @brief Send a config_led_ring_table_packet message + * @param chan MAVLink channel to send the message + * + * @param packet_id Data packet ID. + * @param led_rotation_period Rotation period. + * @param led_zero_point_offset Zero offset unit in milliseconds. + * @param led_display_mode LED display mode selection. + * @param led_table LED display field data packet, 45 bytes represent the status of 360 LEDs bit by bit. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_config_led_ring_table_packet_send(mavlink_channel_t chan, uint16_t packet_id, uint32_t led_rotation_period, uint32_t led_zero_point_offset, uint8_t led_display_mode, const uint8_t *led_table) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN]; + _mav_put_uint32_t(buf, 0, led_rotation_period); + _mav_put_uint32_t(buf, 4, led_zero_point_offset); + _mav_put_uint16_t(buf, 8, packet_id); + _mav_put_uint8_t(buf, 10, led_display_mode); + _mav_put_uint8_t_array(buf, 11, led_table, 45); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET, buf, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +#else + mavlink_config_led_ring_table_packet_t packet; + packet.led_rotation_period = led_rotation_period; + packet.led_zero_point_offset = led_zero_point_offset; + packet.packet_id = packet_id; + packet.led_display_mode = led_display_mode; + mav_array_memcpy(packet.led_table, led_table, sizeof(uint8_t)*45); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET, (const char *)&packet, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +#endif +} + +/** + * @brief Send a config_led_ring_table_packet message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_config_led_ring_table_packet_send_struct(mavlink_channel_t chan, const mavlink_config_led_ring_table_packet_t* config_led_ring_table_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_config_led_ring_table_packet_send(chan, config_led_ring_table_packet->packet_id, config_led_ring_table_packet->led_rotation_period, config_led_ring_table_packet->led_zero_point_offset, config_led_ring_table_packet->led_display_mode, config_led_ring_table_packet->led_table); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET, (const char *)config_led_ring_table_packet, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_config_led_ring_table_packet_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t packet_id, uint32_t led_rotation_period, uint32_t led_zero_point_offset, uint8_t led_display_mode, const uint8_t *led_table) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, led_rotation_period); + _mav_put_uint32_t(buf, 4, led_zero_point_offset); + _mav_put_uint16_t(buf, 8, packet_id); + _mav_put_uint8_t(buf, 10, led_display_mode); + _mav_put_uint8_t_array(buf, 11, led_table, 45); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET, buf, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +#else + mavlink_config_led_ring_table_packet_t *packet = (mavlink_config_led_ring_table_packet_t *)msgbuf; + packet->led_rotation_period = led_rotation_period; + packet->led_zero_point_offset = led_zero_point_offset; + packet->packet_id = packet_id; + packet->led_display_mode = led_display_mode; + mav_array_memcpy(packet->led_table, led_table, sizeof(uint8_t)*45); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET, (const char *)packet, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CONFIG_LED_RING_TABLE_PACKET UNPACKING + + +/** + * @brief Get field packet_id from config_led_ring_table_packet message + * + * @return Data packet ID. + */ +static inline uint16_t mavlink_msg_config_led_ring_table_packet_get_packet_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field led_rotation_period from config_led_ring_table_packet message + * + * @return Rotation period. + */ +static inline uint32_t mavlink_msg_config_led_ring_table_packet_get_led_rotation_period(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field led_zero_point_offset from config_led_ring_table_packet message + * + * @return Zero offset unit in milliseconds. + */ +static inline uint32_t mavlink_msg_config_led_ring_table_packet_get_led_zero_point_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field led_display_mode from config_led_ring_table_packet message + * + * @return LED display mode selection. + */ +static inline uint8_t mavlink_msg_config_led_ring_table_packet_get_led_display_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field led_table from config_led_ring_table_packet message + * + * @return LED display field data packet, 45 bytes represent the status of 360 LEDs bit by bit. + */ +static inline uint16_t mavlink_msg_config_led_ring_table_packet_get_led_table(const mavlink_message_t* msg, uint8_t *led_table) +{ + return _MAV_RETURN_uint8_t_array(msg, led_table, 45, 11); +} + +/** + * @brief Decode a config_led_ring_table_packet message into a struct + * + * @param msg The message to decode + * @param config_led_ring_table_packet C-struct to decode the message contents into + */ +static inline void mavlink_msg_config_led_ring_table_packet_decode(const mavlink_message_t* msg, mavlink_config_led_ring_table_packet_t* config_led_ring_table_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + config_led_ring_table_packet->led_rotation_period = mavlink_msg_config_led_ring_table_packet_get_led_rotation_period(msg); + config_led_ring_table_packet->led_zero_point_offset = mavlink_msg_config_led_ring_table_packet_get_led_zero_point_offset(msg); + config_led_ring_table_packet->packet_id = mavlink_msg_config_led_ring_table_packet_get_packet_id(msg); + config_led_ring_table_packet->led_display_mode = mavlink_msg_config_led_ring_table_packet_get_led_display_mode(msg); + mavlink_msg_config_led_ring_table_packet_get_led_table(msg, config_led_ring_table_packet->led_table); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN? msg->len : MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN; + memset(config_led_ring_table_packet, 0, MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_LEN); + memcpy(config_led_ring_table_packet, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_config_lidar_working_mode.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_config_lidar_working_mode.h new file mode 100644 index 0000000..1cf08c2 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_config_lidar_working_mode.h @@ -0,0 +1,213 @@ +#pragma once +// MESSAGE CONFIG_LIDAR_WORKING_MODE PACKING + +#define MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE 14 + + +typedef struct __mavlink_config_lidar_working_mode_t { + uint8_t request_type; /*< Types of operating modes for LiDAR.*/ +} mavlink_config_lidar_working_mode_t; + +#define MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN 1 +#define MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN 1 +#define MAVLINK_MSG_ID_14_LEN 1 +#define MAVLINK_MSG_ID_14_MIN_LEN 1 + +#define MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC 84 +#define MAVLINK_MSG_ID_14_CRC 84 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CONFIG_LIDAR_WORKING_MODE { \ + 14, \ + "CONFIG_LIDAR_WORKING_MODE", \ + 1, \ + { { "request_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_config_lidar_working_mode_t, request_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CONFIG_LIDAR_WORKING_MODE { \ + "CONFIG_LIDAR_WORKING_MODE", \ + 1, \ + { { "request_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_config_lidar_working_mode_t, request_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a config_lidar_working_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param request_type Types of operating modes for LiDAR. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_config_lidar_working_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN]; + _mav_put_uint8_t(buf, 0, request_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN); +#else + mavlink_config_lidar_working_mode_t packet; + packet.request_type = request_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +} + +/** + * @brief Pack a config_lidar_working_mode message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_type Types of operating modes for LiDAR. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_config_lidar_working_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN]; + _mav_put_uint8_t(buf, 0, request_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN); +#else + mavlink_config_lidar_working_mode_t packet; + packet.request_type = request_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +} + +/** + * @brief Encode a config_lidar_working_mode 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 config_lidar_working_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_config_lidar_working_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_config_lidar_working_mode_t* config_lidar_working_mode) +{ + return mavlink_msg_config_lidar_working_mode_pack(system_id, component_id, msg, config_lidar_working_mode->request_type); +} + +/** + * @brief Encode a config_lidar_working_mode 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 config_lidar_working_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_config_lidar_working_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_config_lidar_working_mode_t* config_lidar_working_mode) +{ + return mavlink_msg_config_lidar_working_mode_pack_chan(system_id, component_id, chan, msg, config_lidar_working_mode->request_type); +} + +/** + * @brief Send a config_lidar_working_mode message + * @param chan MAVLink channel to send the message + * + * @param request_type Types of operating modes for LiDAR. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_config_lidar_working_mode_send(mavlink_channel_t chan, uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN]; + _mav_put_uint8_t(buf, 0, request_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE, buf, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +#else + mavlink_config_lidar_working_mode_t packet; + packet.request_type = request_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE, (const char *)&packet, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +#endif +} + +/** + * @brief Send a config_lidar_working_mode message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_config_lidar_working_mode_send_struct(mavlink_channel_t chan, const mavlink_config_lidar_working_mode_t* config_lidar_working_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_config_lidar_working_mode_send(chan, config_lidar_working_mode->request_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE, (const char *)config_lidar_working_mode, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_config_lidar_working_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, request_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE, buf, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +#else + mavlink_config_lidar_working_mode_t *packet = (mavlink_config_lidar_working_mode_t *)msgbuf; + packet->request_type = request_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE, (const char *)packet, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CONFIG_LIDAR_WORKING_MODE UNPACKING + + +/** + * @brief Get field request_type from config_lidar_working_mode message + * + * @return Types of operating modes for LiDAR. + */ +static inline uint8_t mavlink_msg_config_lidar_working_mode_get_request_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Decode a config_lidar_working_mode message into a struct + * + * @param msg The message to decode + * @param config_lidar_working_mode C-struct to decode the message contents into + */ +static inline void mavlink_msg_config_lidar_working_mode_decode(const mavlink_message_t* msg, mavlink_config_lidar_working_mode_t* config_lidar_working_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + config_lidar_working_mode->request_type = mavlink_msg_config_lidar_working_mode_get_request_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN? msg->len : MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN; + memset(config_lidar_working_mode, 0, MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_LEN); + memcpy(config_lidar_working_mode, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_device_command.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_device_command.h new file mode 100644 index 0000000..a6ef406 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_device_command.h @@ -0,0 +1,213 @@ +#pragma once +// MESSAGE DEVICE_COMMAND PACKING + +#define MAVLINK_MSG_ID_DEVICE_COMMAND 12 + + +typedef struct __mavlink_device_command_t { + uint8_t cmd_type; /*< Command*/ +} mavlink_device_command_t; + +#define MAVLINK_MSG_ID_DEVICE_COMMAND_LEN 1 +#define MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN 1 +#define MAVLINK_MSG_ID_12_LEN 1 +#define MAVLINK_MSG_ID_12_MIN_LEN 1 + +#define MAVLINK_MSG_ID_DEVICE_COMMAND_CRC 204 +#define MAVLINK_MSG_ID_12_CRC 204 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_DEVICE_COMMAND { \ + 12, \ + "DEVICE_COMMAND", \ + 1, \ + { { "cmd_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_device_command_t, cmd_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_DEVICE_COMMAND { \ + "DEVICE_COMMAND", \ + 1, \ + { { "cmd_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_device_command_t, cmd_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a device_command message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param cmd_type Command + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_device_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t cmd_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEVICE_COMMAND_LEN]; + _mav_put_uint8_t(buf, 0, cmd_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN); +#else + mavlink_device_command_t packet; + packet.cmd_type = cmd_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEVICE_COMMAND; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +} + +/** + * @brief Pack a device_command message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_type Command + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_device_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t cmd_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEVICE_COMMAND_LEN]; + _mav_put_uint8_t(buf, 0, cmd_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN); +#else + mavlink_device_command_t packet; + packet.cmd_type = cmd_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEVICE_COMMAND; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +} + +/** + * @brief Encode a device_command 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 device_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_device_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_device_command_t* device_command) +{ + return mavlink_msg_device_command_pack(system_id, component_id, msg, device_command->cmd_type); +} + +/** + * @brief Encode a device_command 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 device_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_device_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_device_command_t* device_command) +{ + return mavlink_msg_device_command_pack_chan(system_id, component_id, chan, msg, device_command->cmd_type); +} + +/** + * @brief Send a device_command message + * @param chan MAVLink channel to send the message + * + * @param cmd_type Command + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_device_command_send(mavlink_channel_t chan, uint8_t cmd_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEVICE_COMMAND_LEN]; + _mav_put_uint8_t(buf, 0, cmd_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_COMMAND, buf, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +#else + mavlink_device_command_t packet; + packet.cmd_type = cmd_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_COMMAND, (const char *)&packet, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +#endif +} + +/** + * @brief Send a device_command message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_device_command_send_struct(mavlink_channel_t chan, const mavlink_device_command_t* device_command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_device_command_send(chan, device_command->cmd_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_COMMAND, (const char *)device_command, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +#endif +} + +#if MAVLINK_MSG_ID_DEVICE_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_device_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t cmd_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, cmd_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_COMMAND, buf, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +#else + mavlink_device_command_t *packet = (mavlink_device_command_t *)msgbuf; + packet->cmd_type = cmd_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_COMMAND, (const char *)packet, MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN, MAVLINK_MSG_ID_DEVICE_COMMAND_CRC); +#endif +} +#endif + +#endif + +// MESSAGE DEVICE_COMMAND UNPACKING + + +/** + * @brief Get field cmd_type from device_command message + * + * @return Command + */ +static inline uint8_t mavlink_msg_device_command_get_cmd_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Decode a device_command message into a struct + * + * @param msg The message to decode + * @param device_command C-struct to decode the message contents into + */ +static inline void mavlink_msg_device_command_decode(const mavlink_message_t* msg, mavlink_device_command_t* device_command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + device_command->cmd_type = mavlink_msg_device_command_get_cmd_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_DEVICE_COMMAND_LEN? msg->len : MAVLINK_MSG_ID_DEVICE_COMMAND_LEN; + memset(device_command, 0, MAVLINK_MSG_ID_DEVICE_COMMAND_LEN); + memcpy(device_command, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_device_request_data.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_device_request_data.h new file mode 100644 index 0000000..903118f --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_device_request_data.h @@ -0,0 +1,213 @@ +#pragma once +// MESSAGE DEVICE_REQUEST_DATA PACKING + +#define MAVLINK_MSG_ID_DEVICE_REQUEST_DATA 11 + + +typedef struct __mavlink_device_request_data_t { + uint8_t request_type; /*< Type of the needed data*/ +} mavlink_device_request_data_t; + +#define MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN 1 +#define MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN 1 +#define MAVLINK_MSG_ID_11_LEN 1 +#define MAVLINK_MSG_ID_11_MIN_LEN 1 + +#define MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC 106 +#define MAVLINK_MSG_ID_11_CRC 106 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_DEVICE_REQUEST_DATA { \ + 11, \ + "DEVICE_REQUEST_DATA", \ + 1, \ + { { "request_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_device_request_data_t, request_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_DEVICE_REQUEST_DATA { \ + "DEVICE_REQUEST_DATA", \ + 1, \ + { { "request_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_device_request_data_t, request_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a device_request_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param request_type Type of the needed data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_device_request_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN]; + _mav_put_uint8_t(buf, 0, request_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN); +#else + mavlink_device_request_data_t packet; + packet.request_type = request_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEVICE_REQUEST_DATA; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +} + +/** + * @brief Pack a device_request_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_type Type of the needed data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_device_request_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN]; + _mav_put_uint8_t(buf, 0, request_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN); +#else + mavlink_device_request_data_t packet; + packet.request_type = request_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEVICE_REQUEST_DATA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +} + +/** + * @brief Encode a device_request_data 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 device_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_device_request_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_device_request_data_t* device_request_data) +{ + return mavlink_msg_device_request_data_pack(system_id, component_id, msg, device_request_data->request_type); +} + +/** + * @brief Encode a device_request_data 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 device_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_device_request_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_device_request_data_t* device_request_data) +{ + return mavlink_msg_device_request_data_pack_chan(system_id, component_id, chan, msg, device_request_data->request_type); +} + +/** + * @brief Send a device_request_data message + * @param chan MAVLink channel to send the message + * + * @param request_type Type of the needed data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_device_request_data_send(mavlink_channel_t chan, uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN]; + _mav_put_uint8_t(buf, 0, request_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA, buf, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +#else + mavlink_device_request_data_t packet; + packet.request_type = request_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +#endif +} + +/** + * @brief Send a device_request_data message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_device_request_data_send_struct(mavlink_channel_t chan, const mavlink_device_request_data_t* device_request_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_device_request_data_send(chan, device_request_data->request_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA, (const char *)device_request_data, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +#endif +} + +#if MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_device_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t request_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, request_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA, buf, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +#else + mavlink_device_request_data_t *packet = (mavlink_device_request_data_t *)msgbuf; + packet->request_type = request_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_CRC); +#endif +} +#endif + +#endif + +// MESSAGE DEVICE_REQUEST_DATA UNPACKING + + +/** + * @brief Get field request_type from device_request_data message + * + * @return Type of the needed data + */ +static inline uint8_t mavlink_msg_device_request_data_get_request_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Decode a device_request_data message into a struct + * + * @param msg The message to decode + * @param device_request_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_device_request_data_decode(const mavlink_message_t* msg, mavlink_device_request_data_t* device_request_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + device_request_data->request_type = mavlink_msg_device_request_data_get_request_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN? msg->len : MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN; + memset(device_request_data, 0, MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_LEN); + memcpy(device_request_data, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_imu_attitude_data_packet.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_imu_attitude_data_packet.h new file mode 100644 index 0000000..a738bd1 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_imu_attitude_data_packet.h @@ -0,0 +1,282 @@ +#pragma once +// MESSAGE RET_IMU_ATTITUDE_DATA_PACKET PACKING + +#define MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET 19 + + +typedef struct __mavlink_ret_imu_attitude_data_packet_t { + float quaternion[4]; /*< Quaternion Array.*/ + float angular_velocity[3]; /*< Three-axis angular velocity values.*/ + float linear_acceleration[3]; /*< Three-axis acceleration values.*/ + uint16_t packet_id; /*< Data packet ID.*/ +} mavlink_ret_imu_attitude_data_packet_t; + +#define MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN 42 +#define MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN 42 +#define MAVLINK_MSG_ID_19_LEN 42 +#define MAVLINK_MSG_ID_19_MIN_LEN 42 + +#define MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC 110 +#define MAVLINK_MSG_ID_19_CRC 110 + +#define MAVLINK_MSG_RET_IMU_ATTITUDE_DATA_PACKET_FIELD_QUATERNION_LEN 4 +#define MAVLINK_MSG_RET_IMU_ATTITUDE_DATA_PACKET_FIELD_ANGULAR_VELOCITY_LEN 3 +#define MAVLINK_MSG_RET_IMU_ATTITUDE_DATA_PACKET_FIELD_LINEAR_ACCELERATION_LEN 3 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RET_IMU_ATTITUDE_DATA_PACKET { \ + 19, \ + "RET_IMU_ATTITUDE_DATA_PACKET", \ + 4, \ + { { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_ret_imu_attitude_data_packet_t, packet_id) }, \ + { "quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 0, offsetof(mavlink_ret_imu_attitude_data_packet_t, quaternion) }, \ + { "angular_velocity", NULL, MAVLINK_TYPE_FLOAT, 3, 16, offsetof(mavlink_ret_imu_attitude_data_packet_t, angular_velocity) }, \ + { "linear_acceleration", NULL, MAVLINK_TYPE_FLOAT, 3, 28, offsetof(mavlink_ret_imu_attitude_data_packet_t, linear_acceleration) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RET_IMU_ATTITUDE_DATA_PACKET { \ + "RET_IMU_ATTITUDE_DATA_PACKET", \ + 4, \ + { { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_ret_imu_attitude_data_packet_t, packet_id) }, \ + { "quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 0, offsetof(mavlink_ret_imu_attitude_data_packet_t, quaternion) }, \ + { "angular_velocity", NULL, MAVLINK_TYPE_FLOAT, 3, 16, offsetof(mavlink_ret_imu_attitude_data_packet_t, angular_velocity) }, \ + { "linear_acceleration", NULL, MAVLINK_TYPE_FLOAT, 3, 28, offsetof(mavlink_ret_imu_attitude_data_packet_t, linear_acceleration) }, \ + } \ +} +#endif + +/** + * @brief Pack a ret_imu_attitude_data_packet 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 packet_id Data packet ID. + * @param quaternion Quaternion Array. + * @param angular_velocity Three-axis angular velocity values. + * @param linear_acceleration Three-axis acceleration values. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t packet_id, const float *quaternion, const float *angular_velocity, const float *linear_acceleration) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN]; + _mav_put_uint16_t(buf, 40, packet_id); + _mav_put_float_array(buf, 0, quaternion, 4); + _mav_put_float_array(buf, 16, angular_velocity, 3); + _mav_put_float_array(buf, 28, linear_acceleration, 3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN); +#else + mavlink_ret_imu_attitude_data_packet_t packet; + packet.packet_id = packet_id; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + mav_array_memcpy(packet.angular_velocity, angular_velocity, sizeof(float)*3); + mav_array_memcpy(packet.linear_acceleration, linear_acceleration, sizeof(float)*3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +} + +/** + * @brief Pack a ret_imu_attitude_data_packet 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 packet_id Data packet ID. + * @param quaternion Quaternion Array. + * @param angular_velocity Three-axis angular velocity values. + * @param linear_acceleration Three-axis acceleration values. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t packet_id,const float *quaternion,const float *angular_velocity,const float *linear_acceleration) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN]; + _mav_put_uint16_t(buf, 40, packet_id); + _mav_put_float_array(buf, 0, quaternion, 4); + _mav_put_float_array(buf, 16, angular_velocity, 3); + _mav_put_float_array(buf, 28, linear_acceleration, 3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN); +#else + mavlink_ret_imu_attitude_data_packet_t packet; + packet.packet_id = packet_id; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + mav_array_memcpy(packet.angular_velocity, angular_velocity, sizeof(float)*3); + mav_array_memcpy(packet.linear_acceleration, linear_acceleration, sizeof(float)*3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +} + +/** + * @brief Encode a ret_imu_attitude_data_packet 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 ret_imu_attitude_data_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ret_imu_attitude_data_packet_t* ret_imu_attitude_data_packet) +{ + return mavlink_msg_ret_imu_attitude_data_packet_pack(system_id, component_id, msg, ret_imu_attitude_data_packet->packet_id, ret_imu_attitude_data_packet->quaternion, ret_imu_attitude_data_packet->angular_velocity, ret_imu_attitude_data_packet->linear_acceleration); +} + +/** + * @brief Encode a ret_imu_attitude_data_packet 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 ret_imu_attitude_data_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ret_imu_attitude_data_packet_t* ret_imu_attitude_data_packet) +{ + return mavlink_msg_ret_imu_attitude_data_packet_pack_chan(system_id, component_id, chan, msg, ret_imu_attitude_data_packet->packet_id, ret_imu_attitude_data_packet->quaternion, ret_imu_attitude_data_packet->angular_velocity, ret_imu_attitude_data_packet->linear_acceleration); +} + +/** + * @brief Send a ret_imu_attitude_data_packet message + * @param chan MAVLink channel to send the message + * + * @param packet_id Data packet ID. + * @param quaternion Quaternion Array. + * @param angular_velocity Three-axis angular velocity values. + * @param linear_acceleration Three-axis acceleration values. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ret_imu_attitude_data_packet_send(mavlink_channel_t chan, uint16_t packet_id, const float *quaternion, const float *angular_velocity, const float *linear_acceleration) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN]; + _mav_put_uint16_t(buf, 40, packet_id); + _mav_put_float_array(buf, 0, quaternion, 4); + _mav_put_float_array(buf, 16, angular_velocity, 3); + _mav_put_float_array(buf, 28, linear_acceleration, 3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET, buf, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +#else + mavlink_ret_imu_attitude_data_packet_t packet; + packet.packet_id = packet_id; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + mav_array_memcpy(packet.angular_velocity, angular_velocity, sizeof(float)*3); + mav_array_memcpy(packet.linear_acceleration, linear_acceleration, sizeof(float)*3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET, (const char *)&packet, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +#endif +} + +/** + * @brief Send a ret_imu_attitude_data_packet message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ret_imu_attitude_data_packet_send_struct(mavlink_channel_t chan, const mavlink_ret_imu_attitude_data_packet_t* ret_imu_attitude_data_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_imu_attitude_data_packet_send(chan, ret_imu_attitude_data_packet->packet_id, ret_imu_attitude_data_packet->quaternion, ret_imu_attitude_data_packet->angular_velocity, ret_imu_attitude_data_packet->linear_acceleration); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET, (const char *)ret_imu_attitude_data_packet, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_ret_imu_attitude_data_packet_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t packet_id, const float *quaternion, const float *angular_velocity, const float *linear_acceleration) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 40, packet_id); + _mav_put_float_array(buf, 0, quaternion, 4); + _mav_put_float_array(buf, 16, angular_velocity, 3); + _mav_put_float_array(buf, 28, linear_acceleration, 3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET, buf, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +#else + mavlink_ret_imu_attitude_data_packet_t *packet = (mavlink_ret_imu_attitude_data_packet_t *)msgbuf; + packet->packet_id = packet_id; + mav_array_memcpy(packet->quaternion, quaternion, sizeof(float)*4); + mav_array_memcpy(packet->angular_velocity, angular_velocity, sizeof(float)*3); + mav_array_memcpy(packet->linear_acceleration, linear_acceleration, sizeof(float)*3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET, (const char *)packet, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RET_IMU_ATTITUDE_DATA_PACKET UNPACKING + + +/** + * @brief Get field packet_id from ret_imu_attitude_data_packet message + * + * @return Data packet ID. + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_get_packet_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 40); +} + +/** + * @brief Get field quaternion from ret_imu_attitude_data_packet message + * + * @return Quaternion Array. + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_get_quaternion(const mavlink_message_t* msg, float *quaternion) +{ + return _MAV_RETURN_float_array(msg, quaternion, 4, 0); +} + +/** + * @brief Get field angular_velocity from ret_imu_attitude_data_packet message + * + * @return Three-axis angular velocity values. + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_get_angular_velocity(const mavlink_message_t* msg, float *angular_velocity) +{ + return _MAV_RETURN_float_array(msg, angular_velocity, 3, 16); +} + +/** + * @brief Get field linear_acceleration from ret_imu_attitude_data_packet message + * + * @return Three-axis acceleration values. + */ +static inline uint16_t mavlink_msg_ret_imu_attitude_data_packet_get_linear_acceleration(const mavlink_message_t* msg, float *linear_acceleration) +{ + return _MAV_RETURN_float_array(msg, linear_acceleration, 3, 28); +} + +/** + * @brief Decode a ret_imu_attitude_data_packet message into a struct + * + * @param msg The message to decode + * @param ret_imu_attitude_data_packet C-struct to decode the message contents into + */ +static inline void mavlink_msg_ret_imu_attitude_data_packet_decode(const mavlink_message_t* msg, mavlink_ret_imu_attitude_data_packet_t* ret_imu_attitude_data_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_imu_attitude_data_packet_get_quaternion(msg, ret_imu_attitude_data_packet->quaternion); + mavlink_msg_ret_imu_attitude_data_packet_get_angular_velocity(msg, ret_imu_attitude_data_packet->angular_velocity); + mavlink_msg_ret_imu_attitude_data_packet_get_linear_acceleration(msg, ret_imu_attitude_data_packet->linear_acceleration); + ret_imu_attitude_data_packet->packet_id = mavlink_msg_ret_imu_attitude_data_packet_get_packet_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN? msg->len : MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN; + memset(ret_imu_attitude_data_packet, 0, MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_LEN); + memcpy(ret_imu_attitude_data_packet, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_auxiliary_data_packet.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_auxiliary_data_packet.h new file mode 100644 index 0000000..9a811e2 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_auxiliary_data_packet.h @@ -0,0 +1,805 @@ +#pragma once +// MESSAGE RET_LIDAR_AUXILIARY_DATA_PACKET PACKING + +#define MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET 17 + + +typedef struct __mavlink_ret_lidar_auxiliary_data_packet_t { + uint32_t lidar_sync_delay_time; /*< Delay time of upper computer communication(us).*/ + uint32_t time_stamp_s_step; /*< LiDAR operational timestamp in seconds.*/ + uint32_t time_stamp_us_step; /*< LiDAR operational timestamp in microsecond .*/ + uint32_t sys_rotation_period; /*< LiDAR low-speed motor rotation period in microsecond .*/ + uint32_t com_rotation_period; /*< LiDAR high-speed motor rotation period in microsecond .*/ + float com_horizontal_angle_start; /*< The first data in the current data packet is the horizontal angle,Distance data applicable to RET_LIDAR_DISTANCE_DATA_PACKET data packet and reflectivity data of RET_LIDAR_AUXILIARY_DATA_PACKET data packet.*/ + float com_horizontal_angle_step; /*< Angle interval between two adjacent data in the horizontal direction.*/ + float sys_vertical_angle_start; /*< The vertical angle of the first data in the current data packet.*/ + float sys_vertical_angle_span; /*< The angular span between adjacent data in the vertical direction of the current data packet.*/ + float apd_temperature; /*< APD temperature value*/ + float dirty_index; /*< Protective cover dirt detection index*/ + float imu_temperature; /*< IMU temperature value*/ + float up_optical_q; /*< Up optical communication quality.*/ + float down_optical_q; /*< Downlink optical communication quality.*/ + float apd_voltage; /*< APD voltage value.*/ + float imu_angle_x_offset; /*< IMU x-axis installation angle error.*/ + float imu_angle_y_offset; /*< IMU y-axis installation angle error.*/ + float imu_angle_z_offset; /*< IMU z-axis installation angle error.*/ + float b_axis_dist; /*< Laser calibration b distance compensation.*/ + float theta_angle; /*< Laser calibration theta angle compensation.*/ + float ksi_angle; /*< Laser calibration ksi angle compensation.*/ + uint16_t packet_id; /*< Data packet ID.*/ + uint16_t payload_size; /*< Data packet payload size.*/ + uint8_t lidar_work_status; /*< LiDAR operational state indicator.*/ + uint8_t reflect_data[120]; /*< reflectivity data*/ +} mavlink_ret_lidar_auxiliary_data_packet_t; + +#define MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN 209 +#define MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN 209 +#define MAVLINK_MSG_ID_17_LEN 209 +#define MAVLINK_MSG_ID_17_MIN_LEN 209 + +#define MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC 99 +#define MAVLINK_MSG_ID_17_CRC 99 + +#define MAVLINK_MSG_RET_LIDAR_AUXILIARY_DATA_PACKET_FIELD_REFLECT_DATA_LEN 120 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_AUXILIARY_DATA_PACKET { \ + 17, \ + "RET_LIDAR_AUXILIARY_DATA_PACKET", \ + 25, \ + { { "lidar_work_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 88, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, lidar_work_status) }, \ + { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, packet_id) }, \ + { "payload_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, payload_size) }, \ + { "lidar_sync_delay_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, lidar_sync_delay_time) }, \ + { "time_stamp_s_step", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, time_stamp_s_step) }, \ + { "time_stamp_us_step", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, time_stamp_us_step) }, \ + { "sys_rotation_period", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, sys_rotation_period) }, \ + { "com_rotation_period", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, com_rotation_period) }, \ + { "com_horizontal_angle_start", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, com_horizontal_angle_start) }, \ + { "com_horizontal_angle_step", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, com_horizontal_angle_step) }, \ + { "sys_vertical_angle_start", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, sys_vertical_angle_start) }, \ + { "sys_vertical_angle_span", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, sys_vertical_angle_span) }, \ + { "apd_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, apd_temperature) }, \ + { "dirty_index", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, dirty_index) }, \ + { "imu_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_temperature) }, \ + { "up_optical_q", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, up_optical_q) }, \ + { "down_optical_q", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, down_optical_q) }, \ + { "apd_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, apd_voltage) }, \ + { "imu_angle_x_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_angle_x_offset) }, \ + { "imu_angle_y_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_angle_y_offset) }, \ + { "imu_angle_z_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_angle_z_offset) }, \ + { "b_axis_dist", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, b_axis_dist) }, \ + { "theta_angle", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, theta_angle) }, \ + { "ksi_angle", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, ksi_angle) }, \ + { "reflect_data", NULL, MAVLINK_TYPE_UINT8_T, 120, 89, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, reflect_data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_AUXILIARY_DATA_PACKET { \ + "RET_LIDAR_AUXILIARY_DATA_PACKET", \ + 25, \ + { { "lidar_work_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 88, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, lidar_work_status) }, \ + { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, packet_id) }, \ + { "payload_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, payload_size) }, \ + { "lidar_sync_delay_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, lidar_sync_delay_time) }, \ + { "time_stamp_s_step", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, time_stamp_s_step) }, \ + { "time_stamp_us_step", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, time_stamp_us_step) }, \ + { "sys_rotation_period", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, sys_rotation_period) }, \ + { "com_rotation_period", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, com_rotation_period) }, \ + { "com_horizontal_angle_start", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, com_horizontal_angle_start) }, \ + { "com_horizontal_angle_step", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, com_horizontal_angle_step) }, \ + { "sys_vertical_angle_start", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, sys_vertical_angle_start) }, \ + { "sys_vertical_angle_span", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, sys_vertical_angle_span) }, \ + { "apd_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, apd_temperature) }, \ + { "dirty_index", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, dirty_index) }, \ + { "imu_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_temperature) }, \ + { "up_optical_q", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, up_optical_q) }, \ + { "down_optical_q", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, down_optical_q) }, \ + { "apd_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, apd_voltage) }, \ + { "imu_angle_x_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_angle_x_offset) }, \ + { "imu_angle_y_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_angle_y_offset) }, \ + { "imu_angle_z_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, imu_angle_z_offset) }, \ + { "b_axis_dist", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, b_axis_dist) }, \ + { "theta_angle", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, theta_angle) }, \ + { "ksi_angle", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, ksi_angle) }, \ + { "reflect_data", NULL, MAVLINK_TYPE_UINT8_T, 120, 89, offsetof(mavlink_ret_lidar_auxiliary_data_packet_t, reflect_data) }, \ + } \ +} +#endif + +/** + * @brief Pack a ret_lidar_auxiliary_data_packet 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 lidar_work_status LiDAR operational state indicator. + * @param packet_id Data packet ID. + * @param payload_size Data packet payload size. + * @param lidar_sync_delay_time Delay time of upper computer communication(us). + * @param time_stamp_s_step LiDAR operational timestamp in seconds. + * @param time_stamp_us_step LiDAR operational timestamp in microsecond . + * @param sys_rotation_period LiDAR low-speed motor rotation period in microsecond . + * @param com_rotation_period LiDAR high-speed motor rotation period in microsecond . + * @param com_horizontal_angle_start The first data in the current data packet is the horizontal angle,Distance data applicable to RET_LIDAR_DISTANCE_DATA_PACKET data packet and reflectivity data of RET_LIDAR_AUXILIARY_DATA_PACKET data packet. + * @param com_horizontal_angle_step Angle interval between two adjacent data in the horizontal direction. + * @param sys_vertical_angle_start The vertical angle of the first data in the current data packet. + * @param sys_vertical_angle_span The angular span between adjacent data in the vertical direction of the current data packet. + * @param apd_temperature APD temperature value + * @param dirty_index Protective cover dirt detection index + * @param imu_temperature IMU temperature value + * @param up_optical_q Up optical communication quality. + * @param down_optical_q Downlink optical communication quality. + * @param apd_voltage APD voltage value. + * @param imu_angle_x_offset IMU x-axis installation angle error. + * @param imu_angle_y_offset IMU y-axis installation angle error. + * @param imu_angle_z_offset IMU z-axis installation angle error. + * @param b_axis_dist Laser calibration b distance compensation. + * @param theta_angle Laser calibration theta angle compensation. + * @param ksi_angle Laser calibration ksi angle compensation. + * @param reflect_data reflectivity data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t lidar_work_status, uint16_t packet_id, uint16_t payload_size, uint32_t lidar_sync_delay_time, uint32_t time_stamp_s_step, uint32_t time_stamp_us_step, uint32_t sys_rotation_period, uint32_t com_rotation_period, float com_horizontal_angle_start, float com_horizontal_angle_step, float sys_vertical_angle_start, float sys_vertical_angle_span, float apd_temperature, float dirty_index, float imu_temperature, float up_optical_q, float down_optical_q, float apd_voltage, float imu_angle_x_offset, float imu_angle_y_offset, float imu_angle_z_offset, float b_axis_dist, float theta_angle, float ksi_angle, const uint8_t *reflect_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN]; + _mav_put_uint32_t(buf, 0, lidar_sync_delay_time); + _mav_put_uint32_t(buf, 4, time_stamp_s_step); + _mav_put_uint32_t(buf, 8, time_stamp_us_step); + _mav_put_uint32_t(buf, 12, sys_rotation_period); + _mav_put_uint32_t(buf, 16, com_rotation_period); + _mav_put_float(buf, 20, com_horizontal_angle_start); + _mav_put_float(buf, 24, com_horizontal_angle_step); + _mav_put_float(buf, 28, sys_vertical_angle_start); + _mav_put_float(buf, 32, sys_vertical_angle_span); + _mav_put_float(buf, 36, apd_temperature); + _mav_put_float(buf, 40, dirty_index); + _mav_put_float(buf, 44, imu_temperature); + _mav_put_float(buf, 48, up_optical_q); + _mav_put_float(buf, 52, down_optical_q); + _mav_put_float(buf, 56, apd_voltage); + _mav_put_float(buf, 60, imu_angle_x_offset); + _mav_put_float(buf, 64, imu_angle_y_offset); + _mav_put_float(buf, 68, imu_angle_z_offset); + _mav_put_float(buf, 72, b_axis_dist); + _mav_put_float(buf, 76, theta_angle); + _mav_put_float(buf, 80, ksi_angle); + _mav_put_uint16_t(buf, 84, packet_id); + _mav_put_uint16_t(buf, 86, payload_size); + _mav_put_uint8_t(buf, 88, lidar_work_status); + _mav_put_uint8_t_array(buf, 89, reflect_data, 120); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN); +#else + mavlink_ret_lidar_auxiliary_data_packet_t packet; + packet.lidar_sync_delay_time = lidar_sync_delay_time; + packet.time_stamp_s_step = time_stamp_s_step; + packet.time_stamp_us_step = time_stamp_us_step; + packet.sys_rotation_period = sys_rotation_period; + packet.com_rotation_period = com_rotation_period; + packet.com_horizontal_angle_start = com_horizontal_angle_start; + packet.com_horizontal_angle_step = com_horizontal_angle_step; + packet.sys_vertical_angle_start = sys_vertical_angle_start; + packet.sys_vertical_angle_span = sys_vertical_angle_span; + packet.apd_temperature = apd_temperature; + packet.dirty_index = dirty_index; + packet.imu_temperature = imu_temperature; + packet.up_optical_q = up_optical_q; + packet.down_optical_q = down_optical_q; + packet.apd_voltage = apd_voltage; + packet.imu_angle_x_offset = imu_angle_x_offset; + packet.imu_angle_y_offset = imu_angle_y_offset; + packet.imu_angle_z_offset = imu_angle_z_offset; + packet.b_axis_dist = b_axis_dist; + packet.theta_angle = theta_angle; + packet.ksi_angle = ksi_angle; + packet.packet_id = packet_id; + packet.payload_size = payload_size; + packet.lidar_work_status = lidar_work_status; + mav_array_memcpy(packet.reflect_data, reflect_data, sizeof(uint8_t)*120); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +} + +/** + * @brief Pack a ret_lidar_auxiliary_data_packet 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 lidar_work_status LiDAR operational state indicator. + * @param packet_id Data packet ID. + * @param payload_size Data packet payload size. + * @param lidar_sync_delay_time Delay time of upper computer communication(us). + * @param time_stamp_s_step LiDAR operational timestamp in seconds. + * @param time_stamp_us_step LiDAR operational timestamp in microsecond . + * @param sys_rotation_period LiDAR low-speed motor rotation period in microsecond . + * @param com_rotation_period LiDAR high-speed motor rotation period in microsecond . + * @param com_horizontal_angle_start The first data in the current data packet is the horizontal angle,Distance data applicable to RET_LIDAR_DISTANCE_DATA_PACKET data packet and reflectivity data of RET_LIDAR_AUXILIARY_DATA_PACKET data packet. + * @param com_horizontal_angle_step Angle interval between two adjacent data in the horizontal direction. + * @param sys_vertical_angle_start The vertical angle of the first data in the current data packet. + * @param sys_vertical_angle_span The angular span between adjacent data in the vertical direction of the current data packet. + * @param apd_temperature APD temperature value + * @param dirty_index Protective cover dirt detection index + * @param imu_temperature IMU temperature value + * @param up_optical_q Up optical communication quality. + * @param down_optical_q Downlink optical communication quality. + * @param apd_voltage APD voltage value. + * @param imu_angle_x_offset IMU x-axis installation angle error. + * @param imu_angle_y_offset IMU y-axis installation angle error. + * @param imu_angle_z_offset IMU z-axis installation angle error. + * @param b_axis_dist Laser calibration b distance compensation. + * @param theta_angle Laser calibration theta angle compensation. + * @param ksi_angle Laser calibration ksi angle compensation. + * @param reflect_data reflectivity data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t lidar_work_status,uint16_t packet_id,uint16_t payload_size,uint32_t lidar_sync_delay_time,uint32_t time_stamp_s_step,uint32_t time_stamp_us_step,uint32_t sys_rotation_period,uint32_t com_rotation_period,float com_horizontal_angle_start,float com_horizontal_angle_step,float sys_vertical_angle_start,float sys_vertical_angle_span,float apd_temperature,float dirty_index,float imu_temperature,float up_optical_q,float down_optical_q,float apd_voltage,float imu_angle_x_offset,float imu_angle_y_offset,float imu_angle_z_offset,float b_axis_dist,float theta_angle,float ksi_angle,const uint8_t *reflect_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN]; + _mav_put_uint32_t(buf, 0, lidar_sync_delay_time); + _mav_put_uint32_t(buf, 4, time_stamp_s_step); + _mav_put_uint32_t(buf, 8, time_stamp_us_step); + _mav_put_uint32_t(buf, 12, sys_rotation_period); + _mav_put_uint32_t(buf, 16, com_rotation_period); + _mav_put_float(buf, 20, com_horizontal_angle_start); + _mav_put_float(buf, 24, com_horizontal_angle_step); + _mav_put_float(buf, 28, sys_vertical_angle_start); + _mav_put_float(buf, 32, sys_vertical_angle_span); + _mav_put_float(buf, 36, apd_temperature); + _mav_put_float(buf, 40, dirty_index); + _mav_put_float(buf, 44, imu_temperature); + _mav_put_float(buf, 48, up_optical_q); + _mav_put_float(buf, 52, down_optical_q); + _mav_put_float(buf, 56, apd_voltage); + _mav_put_float(buf, 60, imu_angle_x_offset); + _mav_put_float(buf, 64, imu_angle_y_offset); + _mav_put_float(buf, 68, imu_angle_z_offset); + _mav_put_float(buf, 72, b_axis_dist); + _mav_put_float(buf, 76, theta_angle); + _mav_put_float(buf, 80, ksi_angle); + _mav_put_uint16_t(buf, 84, packet_id); + _mav_put_uint16_t(buf, 86, payload_size); + _mav_put_uint8_t(buf, 88, lidar_work_status); + _mav_put_uint8_t_array(buf, 89, reflect_data, 120); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN); +#else + mavlink_ret_lidar_auxiliary_data_packet_t packet; + packet.lidar_sync_delay_time = lidar_sync_delay_time; + packet.time_stamp_s_step = time_stamp_s_step; + packet.time_stamp_us_step = time_stamp_us_step; + packet.sys_rotation_period = sys_rotation_period; + packet.com_rotation_period = com_rotation_period; + packet.com_horizontal_angle_start = com_horizontal_angle_start; + packet.com_horizontal_angle_step = com_horizontal_angle_step; + packet.sys_vertical_angle_start = sys_vertical_angle_start; + packet.sys_vertical_angle_span = sys_vertical_angle_span; + packet.apd_temperature = apd_temperature; + packet.dirty_index = dirty_index; + packet.imu_temperature = imu_temperature; + packet.up_optical_q = up_optical_q; + packet.down_optical_q = down_optical_q; + packet.apd_voltage = apd_voltage; + packet.imu_angle_x_offset = imu_angle_x_offset; + packet.imu_angle_y_offset = imu_angle_y_offset; + packet.imu_angle_z_offset = imu_angle_z_offset; + packet.b_axis_dist = b_axis_dist; + packet.theta_angle = theta_angle; + packet.ksi_angle = ksi_angle; + packet.packet_id = packet_id; + packet.payload_size = payload_size; + packet.lidar_work_status = lidar_work_status; + mav_array_memcpy(packet.reflect_data, reflect_data, sizeof(uint8_t)*120); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +} + +/** + * @brief Encode a ret_lidar_auxiliary_data_packet 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 ret_lidar_auxiliary_data_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ret_lidar_auxiliary_data_packet_t* ret_lidar_auxiliary_data_packet) +{ + return mavlink_msg_ret_lidar_auxiliary_data_packet_pack(system_id, component_id, msg, ret_lidar_auxiliary_data_packet->lidar_work_status, ret_lidar_auxiliary_data_packet->packet_id, ret_lidar_auxiliary_data_packet->payload_size, ret_lidar_auxiliary_data_packet->lidar_sync_delay_time, ret_lidar_auxiliary_data_packet->time_stamp_s_step, ret_lidar_auxiliary_data_packet->time_stamp_us_step, ret_lidar_auxiliary_data_packet->sys_rotation_period, ret_lidar_auxiliary_data_packet->com_rotation_period, ret_lidar_auxiliary_data_packet->com_horizontal_angle_start, ret_lidar_auxiliary_data_packet->com_horizontal_angle_step, ret_lidar_auxiliary_data_packet->sys_vertical_angle_start, ret_lidar_auxiliary_data_packet->sys_vertical_angle_span, ret_lidar_auxiliary_data_packet->apd_temperature, ret_lidar_auxiliary_data_packet->dirty_index, ret_lidar_auxiliary_data_packet->imu_temperature, ret_lidar_auxiliary_data_packet->up_optical_q, ret_lidar_auxiliary_data_packet->down_optical_q, ret_lidar_auxiliary_data_packet->apd_voltage, ret_lidar_auxiliary_data_packet->imu_angle_x_offset, ret_lidar_auxiliary_data_packet->imu_angle_y_offset, ret_lidar_auxiliary_data_packet->imu_angle_z_offset, ret_lidar_auxiliary_data_packet->b_axis_dist, ret_lidar_auxiliary_data_packet->theta_angle, ret_lidar_auxiliary_data_packet->ksi_angle, ret_lidar_auxiliary_data_packet->reflect_data); +} + +/** + * @brief Encode a ret_lidar_auxiliary_data_packet 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 ret_lidar_auxiliary_data_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ret_lidar_auxiliary_data_packet_t* ret_lidar_auxiliary_data_packet) +{ + return mavlink_msg_ret_lidar_auxiliary_data_packet_pack_chan(system_id, component_id, chan, msg, ret_lidar_auxiliary_data_packet->lidar_work_status, ret_lidar_auxiliary_data_packet->packet_id, ret_lidar_auxiliary_data_packet->payload_size, ret_lidar_auxiliary_data_packet->lidar_sync_delay_time, ret_lidar_auxiliary_data_packet->time_stamp_s_step, ret_lidar_auxiliary_data_packet->time_stamp_us_step, ret_lidar_auxiliary_data_packet->sys_rotation_period, ret_lidar_auxiliary_data_packet->com_rotation_period, ret_lidar_auxiliary_data_packet->com_horizontal_angle_start, ret_lidar_auxiliary_data_packet->com_horizontal_angle_step, ret_lidar_auxiliary_data_packet->sys_vertical_angle_start, ret_lidar_auxiliary_data_packet->sys_vertical_angle_span, ret_lidar_auxiliary_data_packet->apd_temperature, ret_lidar_auxiliary_data_packet->dirty_index, ret_lidar_auxiliary_data_packet->imu_temperature, ret_lidar_auxiliary_data_packet->up_optical_q, ret_lidar_auxiliary_data_packet->down_optical_q, ret_lidar_auxiliary_data_packet->apd_voltage, ret_lidar_auxiliary_data_packet->imu_angle_x_offset, ret_lidar_auxiliary_data_packet->imu_angle_y_offset, ret_lidar_auxiliary_data_packet->imu_angle_z_offset, ret_lidar_auxiliary_data_packet->b_axis_dist, ret_lidar_auxiliary_data_packet->theta_angle, ret_lidar_auxiliary_data_packet->ksi_angle, ret_lidar_auxiliary_data_packet->reflect_data); +} + +/** + * @brief Send a ret_lidar_auxiliary_data_packet message + * @param chan MAVLink channel to send the message + * + * @param lidar_work_status LiDAR operational state indicator. + * @param packet_id Data packet ID. + * @param payload_size Data packet payload size. + * @param lidar_sync_delay_time Delay time of upper computer communication(us). + * @param time_stamp_s_step LiDAR operational timestamp in seconds. + * @param time_stamp_us_step LiDAR operational timestamp in microsecond . + * @param sys_rotation_period LiDAR low-speed motor rotation period in microsecond . + * @param com_rotation_period LiDAR high-speed motor rotation period in microsecond . + * @param com_horizontal_angle_start The first data in the current data packet is the horizontal angle,Distance data applicable to RET_LIDAR_DISTANCE_DATA_PACKET data packet and reflectivity data of RET_LIDAR_AUXILIARY_DATA_PACKET data packet. + * @param com_horizontal_angle_step Angle interval between two adjacent data in the horizontal direction. + * @param sys_vertical_angle_start The vertical angle of the first data in the current data packet. + * @param sys_vertical_angle_span The angular span between adjacent data in the vertical direction of the current data packet. + * @param apd_temperature APD temperature value + * @param dirty_index Protective cover dirt detection index + * @param imu_temperature IMU temperature value + * @param up_optical_q Up optical communication quality. + * @param down_optical_q Downlink optical communication quality. + * @param apd_voltage APD voltage value. + * @param imu_angle_x_offset IMU x-axis installation angle error. + * @param imu_angle_y_offset IMU y-axis installation angle error. + * @param imu_angle_z_offset IMU z-axis installation angle error. + * @param b_axis_dist Laser calibration b distance compensation. + * @param theta_angle Laser calibration theta angle compensation. + * @param ksi_angle Laser calibration ksi angle compensation. + * @param reflect_data reflectivity data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ret_lidar_auxiliary_data_packet_send(mavlink_channel_t chan, uint8_t lidar_work_status, uint16_t packet_id, uint16_t payload_size, uint32_t lidar_sync_delay_time, uint32_t time_stamp_s_step, uint32_t time_stamp_us_step, uint32_t sys_rotation_period, uint32_t com_rotation_period, float com_horizontal_angle_start, float com_horizontal_angle_step, float sys_vertical_angle_start, float sys_vertical_angle_span, float apd_temperature, float dirty_index, float imu_temperature, float up_optical_q, float down_optical_q, float apd_voltage, float imu_angle_x_offset, float imu_angle_y_offset, float imu_angle_z_offset, float b_axis_dist, float theta_angle, float ksi_angle, const uint8_t *reflect_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN]; + _mav_put_uint32_t(buf, 0, lidar_sync_delay_time); + _mav_put_uint32_t(buf, 4, time_stamp_s_step); + _mav_put_uint32_t(buf, 8, time_stamp_us_step); + _mav_put_uint32_t(buf, 12, sys_rotation_period); + _mav_put_uint32_t(buf, 16, com_rotation_period); + _mav_put_float(buf, 20, com_horizontal_angle_start); + _mav_put_float(buf, 24, com_horizontal_angle_step); + _mav_put_float(buf, 28, sys_vertical_angle_start); + _mav_put_float(buf, 32, sys_vertical_angle_span); + _mav_put_float(buf, 36, apd_temperature); + _mav_put_float(buf, 40, dirty_index); + _mav_put_float(buf, 44, imu_temperature); + _mav_put_float(buf, 48, up_optical_q); + _mav_put_float(buf, 52, down_optical_q); + _mav_put_float(buf, 56, apd_voltage); + _mav_put_float(buf, 60, imu_angle_x_offset); + _mav_put_float(buf, 64, imu_angle_y_offset); + _mav_put_float(buf, 68, imu_angle_z_offset); + _mav_put_float(buf, 72, b_axis_dist); + _mav_put_float(buf, 76, theta_angle); + _mav_put_float(buf, 80, ksi_angle); + _mav_put_uint16_t(buf, 84, packet_id); + _mav_put_uint16_t(buf, 86, payload_size); + _mav_put_uint8_t(buf, 88, lidar_work_status); + _mav_put_uint8_t_array(buf, 89, reflect_data, 120); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET, buf, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +#else + mavlink_ret_lidar_auxiliary_data_packet_t packet; + packet.lidar_sync_delay_time = lidar_sync_delay_time; + packet.time_stamp_s_step = time_stamp_s_step; + packet.time_stamp_us_step = time_stamp_us_step; + packet.sys_rotation_period = sys_rotation_period; + packet.com_rotation_period = com_rotation_period; + packet.com_horizontal_angle_start = com_horizontal_angle_start; + packet.com_horizontal_angle_step = com_horizontal_angle_step; + packet.sys_vertical_angle_start = sys_vertical_angle_start; + packet.sys_vertical_angle_span = sys_vertical_angle_span; + packet.apd_temperature = apd_temperature; + packet.dirty_index = dirty_index; + packet.imu_temperature = imu_temperature; + packet.up_optical_q = up_optical_q; + packet.down_optical_q = down_optical_q; + packet.apd_voltage = apd_voltage; + packet.imu_angle_x_offset = imu_angle_x_offset; + packet.imu_angle_y_offset = imu_angle_y_offset; + packet.imu_angle_z_offset = imu_angle_z_offset; + packet.b_axis_dist = b_axis_dist; + packet.theta_angle = theta_angle; + packet.ksi_angle = ksi_angle; + packet.packet_id = packet_id; + packet.payload_size = payload_size; + packet.lidar_work_status = lidar_work_status; + mav_array_memcpy(packet.reflect_data, reflect_data, sizeof(uint8_t)*120); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET, (const char *)&packet, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +#endif +} + +/** + * @brief Send a ret_lidar_auxiliary_data_packet message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ret_lidar_auxiliary_data_packet_send_struct(mavlink_channel_t chan, const mavlink_ret_lidar_auxiliary_data_packet_t* ret_lidar_auxiliary_data_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_lidar_auxiliary_data_packet_send(chan, ret_lidar_auxiliary_data_packet->lidar_work_status, ret_lidar_auxiliary_data_packet->packet_id, ret_lidar_auxiliary_data_packet->payload_size, ret_lidar_auxiliary_data_packet->lidar_sync_delay_time, ret_lidar_auxiliary_data_packet->time_stamp_s_step, ret_lidar_auxiliary_data_packet->time_stamp_us_step, ret_lidar_auxiliary_data_packet->sys_rotation_period, ret_lidar_auxiliary_data_packet->com_rotation_period, ret_lidar_auxiliary_data_packet->com_horizontal_angle_start, ret_lidar_auxiliary_data_packet->com_horizontal_angle_step, ret_lidar_auxiliary_data_packet->sys_vertical_angle_start, ret_lidar_auxiliary_data_packet->sys_vertical_angle_span, ret_lidar_auxiliary_data_packet->apd_temperature, ret_lidar_auxiliary_data_packet->dirty_index, ret_lidar_auxiliary_data_packet->imu_temperature, ret_lidar_auxiliary_data_packet->up_optical_q, ret_lidar_auxiliary_data_packet->down_optical_q, ret_lidar_auxiliary_data_packet->apd_voltage, ret_lidar_auxiliary_data_packet->imu_angle_x_offset, ret_lidar_auxiliary_data_packet->imu_angle_y_offset, ret_lidar_auxiliary_data_packet->imu_angle_z_offset, ret_lidar_auxiliary_data_packet->b_axis_dist, ret_lidar_auxiliary_data_packet->theta_angle, ret_lidar_auxiliary_data_packet->ksi_angle, ret_lidar_auxiliary_data_packet->reflect_data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET, (const char *)ret_lidar_auxiliary_data_packet, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_ret_lidar_auxiliary_data_packet_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t lidar_work_status, uint16_t packet_id, uint16_t payload_size, uint32_t lidar_sync_delay_time, uint32_t time_stamp_s_step, uint32_t time_stamp_us_step, uint32_t sys_rotation_period, uint32_t com_rotation_period, float com_horizontal_angle_start, float com_horizontal_angle_step, float sys_vertical_angle_start, float sys_vertical_angle_span, float apd_temperature, float dirty_index, float imu_temperature, float up_optical_q, float down_optical_q, float apd_voltage, float imu_angle_x_offset, float imu_angle_y_offset, float imu_angle_z_offset, float b_axis_dist, float theta_angle, float ksi_angle, const uint8_t *reflect_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, lidar_sync_delay_time); + _mav_put_uint32_t(buf, 4, time_stamp_s_step); + _mav_put_uint32_t(buf, 8, time_stamp_us_step); + _mav_put_uint32_t(buf, 12, sys_rotation_period); + _mav_put_uint32_t(buf, 16, com_rotation_period); + _mav_put_float(buf, 20, com_horizontal_angle_start); + _mav_put_float(buf, 24, com_horizontal_angle_step); + _mav_put_float(buf, 28, sys_vertical_angle_start); + _mav_put_float(buf, 32, sys_vertical_angle_span); + _mav_put_float(buf, 36, apd_temperature); + _mav_put_float(buf, 40, dirty_index); + _mav_put_float(buf, 44, imu_temperature); + _mav_put_float(buf, 48, up_optical_q); + _mav_put_float(buf, 52, down_optical_q); + _mav_put_float(buf, 56, apd_voltage); + _mav_put_float(buf, 60, imu_angle_x_offset); + _mav_put_float(buf, 64, imu_angle_y_offset); + _mav_put_float(buf, 68, imu_angle_z_offset); + _mav_put_float(buf, 72, b_axis_dist); + _mav_put_float(buf, 76, theta_angle); + _mav_put_float(buf, 80, ksi_angle); + _mav_put_uint16_t(buf, 84, packet_id); + _mav_put_uint16_t(buf, 86, payload_size); + _mav_put_uint8_t(buf, 88, lidar_work_status); + _mav_put_uint8_t_array(buf, 89, reflect_data, 120); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET, buf, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +#else + mavlink_ret_lidar_auxiliary_data_packet_t *packet = (mavlink_ret_lidar_auxiliary_data_packet_t *)msgbuf; + packet->lidar_sync_delay_time = lidar_sync_delay_time; + packet->time_stamp_s_step = time_stamp_s_step; + packet->time_stamp_us_step = time_stamp_us_step; + packet->sys_rotation_period = sys_rotation_period; + packet->com_rotation_period = com_rotation_period; + packet->com_horizontal_angle_start = com_horizontal_angle_start; + packet->com_horizontal_angle_step = com_horizontal_angle_step; + packet->sys_vertical_angle_start = sys_vertical_angle_start; + packet->sys_vertical_angle_span = sys_vertical_angle_span; + packet->apd_temperature = apd_temperature; + packet->dirty_index = dirty_index; + packet->imu_temperature = imu_temperature; + packet->up_optical_q = up_optical_q; + packet->down_optical_q = down_optical_q; + packet->apd_voltage = apd_voltage; + packet->imu_angle_x_offset = imu_angle_x_offset; + packet->imu_angle_y_offset = imu_angle_y_offset; + packet->imu_angle_z_offset = imu_angle_z_offset; + packet->b_axis_dist = b_axis_dist; + packet->theta_angle = theta_angle; + packet->ksi_angle = ksi_angle; + packet->packet_id = packet_id; + packet->payload_size = payload_size; + packet->lidar_work_status = lidar_work_status; + mav_array_memcpy(packet->reflect_data, reflect_data, sizeof(uint8_t)*120); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET, (const char *)packet, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RET_LIDAR_AUXILIARY_DATA_PACKET UNPACKING + + +/** + * @brief Get field lidar_work_status from ret_lidar_auxiliary_data_packet message + * + * @return LiDAR operational state indicator. + */ +static inline uint8_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_lidar_work_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 88); +} + +/** + * @brief Get field packet_id from ret_lidar_auxiliary_data_packet message + * + * @return Data packet ID. + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_packet_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 84); +} + +/** + * @brief Get field payload_size from ret_lidar_auxiliary_data_packet message + * + * @return Data packet payload size. + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_payload_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 86); +} + +/** + * @brief Get field lidar_sync_delay_time from ret_lidar_auxiliary_data_packet message + * + * @return Delay time of upper computer communication(us). + */ +static inline uint32_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_lidar_sync_delay_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field time_stamp_s_step from ret_lidar_auxiliary_data_packet message + * + * @return LiDAR operational timestamp in seconds. + */ +static inline uint32_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_time_stamp_s_step(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field time_stamp_us_step from ret_lidar_auxiliary_data_packet message + * + * @return LiDAR operational timestamp in microsecond . + */ +static inline uint32_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_time_stamp_us_step(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field sys_rotation_period from ret_lidar_auxiliary_data_packet message + * + * @return LiDAR low-speed motor rotation period in microsecond . + */ +static inline uint32_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_sys_rotation_period(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field com_rotation_period from ret_lidar_auxiliary_data_packet message + * + * @return LiDAR high-speed motor rotation period in microsecond . + */ +static inline uint32_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_com_rotation_period(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 16); +} + +/** + * @brief Get field com_horizontal_angle_start from ret_lidar_auxiliary_data_packet message + * + * @return The first data in the current data packet is the horizontal angle,Distance data applicable to RET_LIDAR_DISTANCE_DATA_PACKET data packet and reflectivity data of RET_LIDAR_AUXILIARY_DATA_PACKET data packet. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_com_horizontal_angle_start(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field com_horizontal_angle_step from ret_lidar_auxiliary_data_packet message + * + * @return Angle interval between two adjacent data in the horizontal direction. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_com_horizontal_angle_step(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field sys_vertical_angle_start from ret_lidar_auxiliary_data_packet message + * + * @return The vertical angle of the first data in the current data packet. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_sys_vertical_angle_start(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field sys_vertical_angle_span from ret_lidar_auxiliary_data_packet message + * + * @return The angular span between adjacent data in the vertical direction of the current data packet. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_sys_vertical_angle_span(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field apd_temperature from ret_lidar_auxiliary_data_packet message + * + * @return APD temperature value + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_apd_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field dirty_index from ret_lidar_auxiliary_data_packet message + * + * @return Protective cover dirt detection index + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_dirty_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field imu_temperature from ret_lidar_auxiliary_data_packet message + * + * @return IMU temperature value + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field up_optical_q from ret_lidar_auxiliary_data_packet message + * + * @return Up optical communication quality. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_up_optical_q(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field down_optical_q from ret_lidar_auxiliary_data_packet message + * + * @return Downlink optical communication quality. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_down_optical_q(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field apd_voltage from ret_lidar_auxiliary_data_packet message + * + * @return APD voltage value. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_apd_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field imu_angle_x_offset from ret_lidar_auxiliary_data_packet message + * + * @return IMU x-axis installation angle error. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_angle_x_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 60); +} + +/** + * @brief Get field imu_angle_y_offset from ret_lidar_auxiliary_data_packet message + * + * @return IMU y-axis installation angle error. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_angle_y_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 64); +} + +/** + * @brief Get field imu_angle_z_offset from ret_lidar_auxiliary_data_packet message + * + * @return IMU z-axis installation angle error. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_angle_z_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 68); +} + +/** + * @brief Get field b_axis_dist from ret_lidar_auxiliary_data_packet message + * + * @return Laser calibration b distance compensation. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_b_axis_dist(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 72); +} + +/** + * @brief Get field theta_angle from ret_lidar_auxiliary_data_packet message + * + * @return Laser calibration theta angle compensation. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_theta_angle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 76); +} + +/** + * @brief Get field ksi_angle from ret_lidar_auxiliary_data_packet message + * + * @return Laser calibration ksi angle compensation. + */ +static inline float mavlink_msg_ret_lidar_auxiliary_data_packet_get_ksi_angle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 80); +} + +/** + * @brief Get field reflect_data from ret_lidar_auxiliary_data_packet message + * + * @return reflectivity data + */ +static inline uint16_t mavlink_msg_ret_lidar_auxiliary_data_packet_get_reflect_data(const mavlink_message_t* msg, uint8_t *reflect_data) +{ + return _MAV_RETURN_uint8_t_array(msg, reflect_data, 120, 89); +} + +/** + * @brief Decode a ret_lidar_auxiliary_data_packet message into a struct + * + * @param msg The message to decode + * @param ret_lidar_auxiliary_data_packet C-struct to decode the message contents into + */ +static inline void mavlink_msg_ret_lidar_auxiliary_data_packet_decode(const mavlink_message_t* msg, mavlink_ret_lidar_auxiliary_data_packet_t* ret_lidar_auxiliary_data_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + ret_lidar_auxiliary_data_packet->lidar_sync_delay_time = mavlink_msg_ret_lidar_auxiliary_data_packet_get_lidar_sync_delay_time(msg); + ret_lidar_auxiliary_data_packet->time_stamp_s_step = mavlink_msg_ret_lidar_auxiliary_data_packet_get_time_stamp_s_step(msg); + ret_lidar_auxiliary_data_packet->time_stamp_us_step = mavlink_msg_ret_lidar_auxiliary_data_packet_get_time_stamp_us_step(msg); + ret_lidar_auxiliary_data_packet->sys_rotation_period = mavlink_msg_ret_lidar_auxiliary_data_packet_get_sys_rotation_period(msg); + ret_lidar_auxiliary_data_packet->com_rotation_period = mavlink_msg_ret_lidar_auxiliary_data_packet_get_com_rotation_period(msg); + ret_lidar_auxiliary_data_packet->com_horizontal_angle_start = mavlink_msg_ret_lidar_auxiliary_data_packet_get_com_horizontal_angle_start(msg); + ret_lidar_auxiliary_data_packet->com_horizontal_angle_step = mavlink_msg_ret_lidar_auxiliary_data_packet_get_com_horizontal_angle_step(msg); + ret_lidar_auxiliary_data_packet->sys_vertical_angle_start = mavlink_msg_ret_lidar_auxiliary_data_packet_get_sys_vertical_angle_start(msg); + ret_lidar_auxiliary_data_packet->sys_vertical_angle_span = mavlink_msg_ret_lidar_auxiliary_data_packet_get_sys_vertical_angle_span(msg); + ret_lidar_auxiliary_data_packet->apd_temperature = mavlink_msg_ret_lidar_auxiliary_data_packet_get_apd_temperature(msg); + ret_lidar_auxiliary_data_packet->dirty_index = mavlink_msg_ret_lidar_auxiliary_data_packet_get_dirty_index(msg); + ret_lidar_auxiliary_data_packet->imu_temperature = mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_temperature(msg); + ret_lidar_auxiliary_data_packet->up_optical_q = mavlink_msg_ret_lidar_auxiliary_data_packet_get_up_optical_q(msg); + ret_lidar_auxiliary_data_packet->down_optical_q = mavlink_msg_ret_lidar_auxiliary_data_packet_get_down_optical_q(msg); + ret_lidar_auxiliary_data_packet->apd_voltage = mavlink_msg_ret_lidar_auxiliary_data_packet_get_apd_voltage(msg); + ret_lidar_auxiliary_data_packet->imu_angle_x_offset = mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_angle_x_offset(msg); + ret_lidar_auxiliary_data_packet->imu_angle_y_offset = mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_angle_y_offset(msg); + ret_lidar_auxiliary_data_packet->imu_angle_z_offset = mavlink_msg_ret_lidar_auxiliary_data_packet_get_imu_angle_z_offset(msg); + ret_lidar_auxiliary_data_packet->b_axis_dist = mavlink_msg_ret_lidar_auxiliary_data_packet_get_b_axis_dist(msg); + ret_lidar_auxiliary_data_packet->theta_angle = mavlink_msg_ret_lidar_auxiliary_data_packet_get_theta_angle(msg); + ret_lidar_auxiliary_data_packet->ksi_angle = mavlink_msg_ret_lidar_auxiliary_data_packet_get_ksi_angle(msg); + ret_lidar_auxiliary_data_packet->packet_id = mavlink_msg_ret_lidar_auxiliary_data_packet_get_packet_id(msg); + ret_lidar_auxiliary_data_packet->payload_size = mavlink_msg_ret_lidar_auxiliary_data_packet_get_payload_size(msg); + ret_lidar_auxiliary_data_packet->lidar_work_status = mavlink_msg_ret_lidar_auxiliary_data_packet_get_lidar_work_status(msg); + mavlink_msg_ret_lidar_auxiliary_data_packet_get_reflect_data(msg, ret_lidar_auxiliary_data_packet->reflect_data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN? msg->len : MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN; + memset(ret_lidar_auxiliary_data_packet, 0, MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_LEN); + memcpy(ret_lidar_auxiliary_data_packet, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_distance_data_packet.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_distance_data_packet.h new file mode 100644 index 0000000..28a7c0a --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_distance_data_packet.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE RET_LIDAR_DISTANCE_DATA_PACKET PACKING + +#define MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET 16 + + +typedef struct __mavlink_ret_lidar_distance_data_packet_t { + uint16_t packet_id; /*< Data packet ID.*/ + uint16_t packet_cnt; /*< Data packet count.*/ + uint16_t payload_size; /*< Data packet payload size.*/ + uint8_t point_data[240]; /*< Data packet distance data.*/ +} mavlink_ret_lidar_distance_data_packet_t; + +#define MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN 246 +#define MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN 246 +#define MAVLINK_MSG_ID_16_LEN 246 +#define MAVLINK_MSG_ID_16_MIN_LEN 246 + +#define MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC 74 +#define MAVLINK_MSG_ID_16_CRC 74 + +#define MAVLINK_MSG_RET_LIDAR_DISTANCE_DATA_PACKET_FIELD_POINT_DATA_LEN 240 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_DISTANCE_DATA_PACKET { \ + 16, \ + "RET_LIDAR_DISTANCE_DATA_PACKET", \ + 4, \ + { { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ret_lidar_distance_data_packet_t, packet_id) }, \ + { "packet_cnt", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ret_lidar_distance_data_packet_t, packet_cnt) }, \ + { "payload_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ret_lidar_distance_data_packet_t, payload_size) }, \ + { "point_data", NULL, MAVLINK_TYPE_UINT8_T, 240, 6, offsetof(mavlink_ret_lidar_distance_data_packet_t, point_data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_DISTANCE_DATA_PACKET { \ + "RET_LIDAR_DISTANCE_DATA_PACKET", \ + 4, \ + { { "packet_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ret_lidar_distance_data_packet_t, packet_id) }, \ + { "packet_cnt", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ret_lidar_distance_data_packet_t, packet_cnt) }, \ + { "payload_size", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ret_lidar_distance_data_packet_t, payload_size) }, \ + { "point_data", NULL, MAVLINK_TYPE_UINT8_T, 240, 6, offsetof(mavlink_ret_lidar_distance_data_packet_t, point_data) }, \ + } \ +} +#endif + +/** + * @brief Pack a ret_lidar_distance_data_packet 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 packet_id Data packet ID. + * @param packet_cnt Data packet count. + * @param payload_size Data packet payload size. + * @param point_data Data packet distance data. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t packet_id, uint16_t packet_cnt, uint16_t payload_size, const uint8_t *point_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN]; + _mav_put_uint16_t(buf, 0, packet_id); + _mav_put_uint16_t(buf, 2, packet_cnt); + _mav_put_uint16_t(buf, 4, payload_size); + _mav_put_uint8_t_array(buf, 6, point_data, 240); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN); +#else + mavlink_ret_lidar_distance_data_packet_t packet; + packet.packet_id = packet_id; + packet.packet_cnt = packet_cnt; + packet.payload_size = payload_size; + mav_array_memcpy(packet.point_data, point_data, sizeof(uint8_t)*240); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +} + +/** + * @brief Pack a ret_lidar_distance_data_packet 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 packet_id Data packet ID. + * @param packet_cnt Data packet count. + * @param payload_size Data packet payload size. + * @param point_data Data packet distance data. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t packet_id,uint16_t packet_cnt,uint16_t payload_size,const uint8_t *point_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN]; + _mav_put_uint16_t(buf, 0, packet_id); + _mav_put_uint16_t(buf, 2, packet_cnt); + _mav_put_uint16_t(buf, 4, payload_size); + _mav_put_uint8_t_array(buf, 6, point_data, 240); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN); +#else + mavlink_ret_lidar_distance_data_packet_t packet; + packet.packet_id = packet_id; + packet.packet_cnt = packet_cnt; + packet.payload_size = payload_size; + mav_array_memcpy(packet.point_data, point_data, sizeof(uint8_t)*240); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +} + +/** + * @brief Encode a ret_lidar_distance_data_packet 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 ret_lidar_distance_data_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ret_lidar_distance_data_packet_t* ret_lidar_distance_data_packet) +{ + return mavlink_msg_ret_lidar_distance_data_packet_pack(system_id, component_id, msg, ret_lidar_distance_data_packet->packet_id, ret_lidar_distance_data_packet->packet_cnt, ret_lidar_distance_data_packet->payload_size, ret_lidar_distance_data_packet->point_data); +} + +/** + * @brief Encode a ret_lidar_distance_data_packet 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 ret_lidar_distance_data_packet C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ret_lidar_distance_data_packet_t* ret_lidar_distance_data_packet) +{ + return mavlink_msg_ret_lidar_distance_data_packet_pack_chan(system_id, component_id, chan, msg, ret_lidar_distance_data_packet->packet_id, ret_lidar_distance_data_packet->packet_cnt, ret_lidar_distance_data_packet->payload_size, ret_lidar_distance_data_packet->point_data); +} + +/** + * @brief Send a ret_lidar_distance_data_packet message + * @param chan MAVLink channel to send the message + * + * @param packet_id Data packet ID. + * @param packet_cnt Data packet count. + * @param payload_size Data packet payload size. + * @param point_data Data packet distance data. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ret_lidar_distance_data_packet_send(mavlink_channel_t chan, uint16_t packet_id, uint16_t packet_cnt, uint16_t payload_size, const uint8_t *point_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN]; + _mav_put_uint16_t(buf, 0, packet_id); + _mav_put_uint16_t(buf, 2, packet_cnt); + _mav_put_uint16_t(buf, 4, payload_size); + _mav_put_uint8_t_array(buf, 6, point_data, 240); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET, buf, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +#else + mavlink_ret_lidar_distance_data_packet_t packet; + packet.packet_id = packet_id; + packet.packet_cnt = packet_cnt; + packet.payload_size = payload_size; + mav_array_memcpy(packet.point_data, point_data, sizeof(uint8_t)*240); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET, (const char *)&packet, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +#endif +} + +/** + * @brief Send a ret_lidar_distance_data_packet message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ret_lidar_distance_data_packet_send_struct(mavlink_channel_t chan, const mavlink_ret_lidar_distance_data_packet_t* ret_lidar_distance_data_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_lidar_distance_data_packet_send(chan, ret_lidar_distance_data_packet->packet_id, ret_lidar_distance_data_packet->packet_cnt, ret_lidar_distance_data_packet->payload_size, ret_lidar_distance_data_packet->point_data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET, (const char *)ret_lidar_distance_data_packet, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_ret_lidar_distance_data_packet_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t packet_id, uint16_t packet_cnt, uint16_t payload_size, const uint8_t *point_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, packet_id); + _mav_put_uint16_t(buf, 2, packet_cnt); + _mav_put_uint16_t(buf, 4, payload_size); + _mav_put_uint8_t_array(buf, 6, point_data, 240); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET, buf, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +#else + mavlink_ret_lidar_distance_data_packet_t *packet = (mavlink_ret_lidar_distance_data_packet_t *)msgbuf; + packet->packet_id = packet_id; + packet->packet_cnt = packet_cnt; + packet->payload_size = payload_size; + mav_array_memcpy(packet->point_data, point_data, sizeof(uint8_t)*240); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET, (const char *)packet, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RET_LIDAR_DISTANCE_DATA_PACKET UNPACKING + + +/** + * @brief Get field packet_id from ret_lidar_distance_data_packet message + * + * @return Data packet ID. + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_get_packet_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field packet_cnt from ret_lidar_distance_data_packet message + * + * @return Data packet count. + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_get_packet_cnt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field payload_size from ret_lidar_distance_data_packet message + * + * @return Data packet payload size. + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_get_payload_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field point_data from ret_lidar_distance_data_packet message + * + * @return Data packet distance data. + */ +static inline uint16_t mavlink_msg_ret_lidar_distance_data_packet_get_point_data(const mavlink_message_t* msg, uint8_t *point_data) +{ + return _MAV_RETURN_uint8_t_array(msg, point_data, 240, 6); +} + +/** + * @brief Decode a ret_lidar_distance_data_packet message into a struct + * + * @param msg The message to decode + * @param ret_lidar_distance_data_packet C-struct to decode the message contents into + */ +static inline void mavlink_msg_ret_lidar_distance_data_packet_decode(const mavlink_message_t* msg, mavlink_ret_lidar_distance_data_packet_t* ret_lidar_distance_data_packet) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + ret_lidar_distance_data_packet->packet_id = mavlink_msg_ret_lidar_distance_data_packet_get_packet_id(msg); + ret_lidar_distance_data_packet->packet_cnt = mavlink_msg_ret_lidar_distance_data_packet_get_packet_cnt(msg); + ret_lidar_distance_data_packet->payload_size = mavlink_msg_ret_lidar_distance_data_packet_get_payload_size(msg); + mavlink_msg_ret_lidar_distance_data_packet_get_point_data(msg, ret_lidar_distance_data_packet->point_data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN? msg->len : MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN; + memset(ret_lidar_distance_data_packet, 0, MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_LEN); + memcpy(ret_lidar_distance_data_packet, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_time_sync_data.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_time_sync_data.h new file mode 100644 index 0000000..2f45aad --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_time_sync_data.h @@ -0,0 +1,238 @@ +#pragma once +// MESSAGE RET_LIDAR_TIME_SYNC_DATA PACKING + +#define MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA 18 + + +typedef struct __mavlink_ret_lidar_time_sync_data_t { + uint32_t lidar_sync_count; /*< Time synchronization packet count.*/ + uint8_t lidar_sync_type; /*< Time synchronization packet type.*/ +} mavlink_ret_lidar_time_sync_data_t; + +#define MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN 5 +#define MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN 5 +#define MAVLINK_MSG_ID_18_LEN 5 +#define MAVLINK_MSG_ID_18_MIN_LEN 5 + +#define MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC 12 +#define MAVLINK_MSG_ID_18_CRC 12 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_TIME_SYNC_DATA { \ + 18, \ + "RET_LIDAR_TIME_SYNC_DATA", \ + 2, \ + { { "lidar_sync_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_ret_lidar_time_sync_data_t, lidar_sync_type) }, \ + { "lidar_sync_count", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ret_lidar_time_sync_data_t, lidar_sync_count) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_TIME_SYNC_DATA { \ + "RET_LIDAR_TIME_SYNC_DATA", \ + 2, \ + { { "lidar_sync_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_ret_lidar_time_sync_data_t, lidar_sync_type) }, \ + { "lidar_sync_count", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ret_lidar_time_sync_data_t, lidar_sync_count) }, \ + } \ +} +#endif + +/** + * @brief Pack a ret_lidar_time_sync_data 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 lidar_sync_type Time synchronization packet type. + * @param lidar_sync_count Time synchronization packet count. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_time_sync_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t lidar_sync_type, uint32_t lidar_sync_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN]; + _mav_put_uint32_t(buf, 0, lidar_sync_count); + _mav_put_uint8_t(buf, 4, lidar_sync_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN); +#else + mavlink_ret_lidar_time_sync_data_t packet; + packet.lidar_sync_count = lidar_sync_count; + packet.lidar_sync_type = lidar_sync_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +} + +/** + * @brief Pack a ret_lidar_time_sync_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param lidar_sync_type Time synchronization packet type. + * @param lidar_sync_count Time synchronization packet count. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_time_sync_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t lidar_sync_type,uint32_t lidar_sync_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN]; + _mav_put_uint32_t(buf, 0, lidar_sync_count); + _mav_put_uint8_t(buf, 4, lidar_sync_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN); +#else + mavlink_ret_lidar_time_sync_data_t packet; + packet.lidar_sync_count = lidar_sync_count; + packet.lidar_sync_type = lidar_sync_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +} + +/** + * @brief Encode a ret_lidar_time_sync_data 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 ret_lidar_time_sync_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_time_sync_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ret_lidar_time_sync_data_t* ret_lidar_time_sync_data) +{ + return mavlink_msg_ret_lidar_time_sync_data_pack(system_id, component_id, msg, ret_lidar_time_sync_data->lidar_sync_type, ret_lidar_time_sync_data->lidar_sync_count); +} + +/** + * @brief Encode a ret_lidar_time_sync_data 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 ret_lidar_time_sync_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_time_sync_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ret_lidar_time_sync_data_t* ret_lidar_time_sync_data) +{ + return mavlink_msg_ret_lidar_time_sync_data_pack_chan(system_id, component_id, chan, msg, ret_lidar_time_sync_data->lidar_sync_type, ret_lidar_time_sync_data->lidar_sync_count); +} + +/** + * @brief Send a ret_lidar_time_sync_data message + * @param chan MAVLink channel to send the message + * + * @param lidar_sync_type Time synchronization packet type. + * @param lidar_sync_count Time synchronization packet count. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ret_lidar_time_sync_data_send(mavlink_channel_t chan, uint8_t lidar_sync_type, uint32_t lidar_sync_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN]; + _mav_put_uint32_t(buf, 0, lidar_sync_count); + _mav_put_uint8_t(buf, 4, lidar_sync_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA, buf, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +#else + mavlink_ret_lidar_time_sync_data_t packet; + packet.lidar_sync_count = lidar_sync_count; + packet.lidar_sync_type = lidar_sync_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA, (const char *)&packet, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +#endif +} + +/** + * @brief Send a ret_lidar_time_sync_data message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ret_lidar_time_sync_data_send_struct(mavlink_channel_t chan, const mavlink_ret_lidar_time_sync_data_t* ret_lidar_time_sync_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_lidar_time_sync_data_send(chan, ret_lidar_time_sync_data->lidar_sync_type, ret_lidar_time_sync_data->lidar_sync_count); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA, (const char *)ret_lidar_time_sync_data, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_ret_lidar_time_sync_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t lidar_sync_type, uint32_t lidar_sync_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, lidar_sync_count); + _mav_put_uint8_t(buf, 4, lidar_sync_type); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA, buf, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +#else + mavlink_ret_lidar_time_sync_data_t *packet = (mavlink_ret_lidar_time_sync_data_t *)msgbuf; + packet->lidar_sync_count = lidar_sync_count; + packet->lidar_sync_type = lidar_sync_type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA, (const char *)packet, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RET_LIDAR_TIME_SYNC_DATA UNPACKING + + +/** + * @brief Get field lidar_sync_type from ret_lidar_time_sync_data message + * + * @return Time synchronization packet type. + */ +static inline uint8_t mavlink_msg_ret_lidar_time_sync_data_get_lidar_sync_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field lidar_sync_count from ret_lidar_time_sync_data message + * + * @return Time synchronization packet count. + */ +static inline uint32_t mavlink_msg_ret_lidar_time_sync_data_get_lidar_sync_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Decode a ret_lidar_time_sync_data message into a struct + * + * @param msg The message to decode + * @param ret_lidar_time_sync_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_ret_lidar_time_sync_data_decode(const mavlink_message_t* msg, mavlink_ret_lidar_time_sync_data_t* ret_lidar_time_sync_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + ret_lidar_time_sync_data->lidar_sync_count = mavlink_msg_ret_lidar_time_sync_data_get_lidar_sync_count(msg); + ret_lidar_time_sync_data->lidar_sync_type = mavlink_msg_ret_lidar_time_sync_data_get_lidar_sync_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN? msg->len : MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN; + memset(ret_lidar_time_sync_data, 0, MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_LEN); + memcpy(ret_lidar_time_sync_data, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_version.h b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_version.h new file mode 100644 index 0000000..bdc1ffa --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/mavlink_msg_ret_lidar_version.h @@ -0,0 +1,291 @@ +#pragma once +// MESSAGE RET_LIDAR_VERSION PACKING + +#define MAVLINK_MSG_ID_RET_LIDAR_VERSION 13 + + +typedef struct __mavlink_ret_lidar_version_t { + uint8_t sys_soft_version[13]; /*< SYS board soft version.*/ + uint8_t com_soft_version[13]; /*< COM board soft version.*/ + uint8_t sys_hard_version[6]; /*< SYS board hard version.*/ + uint8_t com_hard_version[6]; /*< COM board hard version.*/ +} mavlink_ret_lidar_version_t; + +#define MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN 38 +#define MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN 38 +#define MAVLINK_MSG_ID_13_LEN 38 +#define MAVLINK_MSG_ID_13_MIN_LEN 38 + +#define MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC 62 +#define MAVLINK_MSG_ID_13_CRC 62 + +#define MAVLINK_MSG_RET_LIDAR_VERSION_FIELD_SYS_SOFT_VERSION_LEN 13 +#define MAVLINK_MSG_RET_LIDAR_VERSION_FIELD_COM_SOFT_VERSION_LEN 13 +#define MAVLINK_MSG_RET_LIDAR_VERSION_FIELD_SYS_HARD_VERSION_LEN 6 +#define MAVLINK_MSG_RET_LIDAR_VERSION_FIELD_COM_HARD_VERSION_LEN 6 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_VERSION { \ + 13, \ + "RET_LIDAR_VERSION", \ + 4, \ + { { "sys_soft_version", NULL, MAVLINK_TYPE_UINT8_T, 13, 0, offsetof(mavlink_ret_lidar_version_t, sys_soft_version) }, \ + { "com_soft_version", NULL, MAVLINK_TYPE_UINT8_T, 13, 13, offsetof(mavlink_ret_lidar_version_t, com_soft_version) }, \ + { "sys_hard_version", NULL, MAVLINK_TYPE_UINT8_T, 6, 26, offsetof(mavlink_ret_lidar_version_t, sys_hard_version) }, \ + { "com_hard_version", NULL, MAVLINK_TYPE_UINT8_T, 6, 32, offsetof(mavlink_ret_lidar_version_t, com_hard_version) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RET_LIDAR_VERSION { \ + "RET_LIDAR_VERSION", \ + 4, \ + { { "sys_soft_version", NULL, MAVLINK_TYPE_UINT8_T, 13, 0, offsetof(mavlink_ret_lidar_version_t, sys_soft_version) }, \ + { "com_soft_version", NULL, MAVLINK_TYPE_UINT8_T, 13, 13, offsetof(mavlink_ret_lidar_version_t, com_soft_version) }, \ + { "sys_hard_version", NULL, MAVLINK_TYPE_UINT8_T, 6, 26, offsetof(mavlink_ret_lidar_version_t, sys_hard_version) }, \ + { "com_hard_version", NULL, MAVLINK_TYPE_UINT8_T, 6, 32, offsetof(mavlink_ret_lidar_version_t, com_hard_version) }, \ + } \ +} +#endif + +/** + * @brief Pack a ret_lidar_version 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 sys_soft_version SYS board soft version. + * @param com_soft_version COM board soft version. + * @param sys_hard_version SYS board hard version. + * @param com_hard_version COM board hard version. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const uint8_t *sys_soft_version, const uint8_t *com_soft_version, const uint8_t *sys_hard_version, const uint8_t *com_hard_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN]; + + _mav_put_uint8_t_array(buf, 0, sys_soft_version, 13); + _mav_put_uint8_t_array(buf, 13, com_soft_version, 13); + _mav_put_uint8_t_array(buf, 26, sys_hard_version, 6); + _mav_put_uint8_t_array(buf, 32, com_hard_version, 6); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN); +#else + mavlink_ret_lidar_version_t packet; + + mav_array_memcpy(packet.sys_soft_version, sys_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet.com_soft_version, com_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet.sys_hard_version, sys_hard_version, sizeof(uint8_t)*6); + mav_array_memcpy(packet.com_hard_version, com_hard_version, sizeof(uint8_t)*6); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_VERSION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +} + +/** + * @brief Pack a ret_lidar_version 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 sys_soft_version SYS board soft version. + * @param com_soft_version COM board soft version. + * @param sys_hard_version SYS board hard version. + * @param com_hard_version COM board hard version. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ret_lidar_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const uint8_t *sys_soft_version,const uint8_t *com_soft_version,const uint8_t *sys_hard_version,const uint8_t *com_hard_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN]; + + _mav_put_uint8_t_array(buf, 0, sys_soft_version, 13); + _mav_put_uint8_t_array(buf, 13, com_soft_version, 13); + _mav_put_uint8_t_array(buf, 26, sys_hard_version, 6); + _mav_put_uint8_t_array(buf, 32, com_hard_version, 6); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN); +#else + mavlink_ret_lidar_version_t packet; + + mav_array_memcpy(packet.sys_soft_version, sys_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet.com_soft_version, com_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet.sys_hard_version, sys_hard_version, sizeof(uint8_t)*6); + mav_array_memcpy(packet.com_hard_version, com_hard_version, sizeof(uint8_t)*6); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RET_LIDAR_VERSION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +} + +/** + * @brief Encode a ret_lidar_version 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 ret_lidar_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ret_lidar_version_t* ret_lidar_version) +{ + return mavlink_msg_ret_lidar_version_pack(system_id, component_id, msg, ret_lidar_version->sys_soft_version, ret_lidar_version->com_soft_version, ret_lidar_version->sys_hard_version, ret_lidar_version->com_hard_version); +} + +/** + * @brief Encode a ret_lidar_version 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 ret_lidar_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ret_lidar_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ret_lidar_version_t* ret_lidar_version) +{ + return mavlink_msg_ret_lidar_version_pack_chan(system_id, component_id, chan, msg, ret_lidar_version->sys_soft_version, ret_lidar_version->com_soft_version, ret_lidar_version->sys_hard_version, ret_lidar_version->com_hard_version); +} + +/** + * @brief Send a ret_lidar_version message + * @param chan MAVLink channel to send the message + * + * @param sys_soft_version SYS board soft version. + * @param com_soft_version COM board soft version. + * @param sys_hard_version SYS board hard version. + * @param com_hard_version COM board hard version. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ret_lidar_version_send(mavlink_channel_t chan, const uint8_t *sys_soft_version, const uint8_t *com_soft_version, const uint8_t *sys_hard_version, const uint8_t *com_hard_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN]; + + _mav_put_uint8_t_array(buf, 0, sys_soft_version, 13); + _mav_put_uint8_t_array(buf, 13, com_soft_version, 13); + _mav_put_uint8_t_array(buf, 26, sys_hard_version, 6); + _mav_put_uint8_t_array(buf, 32, com_hard_version, 6); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_VERSION, buf, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +#else + mavlink_ret_lidar_version_t packet; + + mav_array_memcpy(packet.sys_soft_version, sys_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet.com_soft_version, com_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet.sys_hard_version, sys_hard_version, sizeof(uint8_t)*6); + mav_array_memcpy(packet.com_hard_version, com_hard_version, sizeof(uint8_t)*6); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_VERSION, (const char *)&packet, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +#endif +} + +/** + * @brief Send a ret_lidar_version message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ret_lidar_version_send_struct(mavlink_channel_t chan, const mavlink_ret_lidar_version_t* ret_lidar_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_lidar_version_send(chan, ret_lidar_version->sys_soft_version, ret_lidar_version->com_soft_version, ret_lidar_version->sys_hard_version, ret_lidar_version->com_hard_version); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_VERSION, (const char *)ret_lidar_version, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant 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_ret_lidar_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sys_soft_version, const uint8_t *com_soft_version, const uint8_t *sys_hard_version, const uint8_t *com_hard_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_uint8_t_array(buf, 0, sys_soft_version, 13); + _mav_put_uint8_t_array(buf, 13, com_soft_version, 13); + _mav_put_uint8_t_array(buf, 26, sys_hard_version, 6); + _mav_put_uint8_t_array(buf, 32, com_hard_version, 6); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_VERSION, buf, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +#else + mavlink_ret_lidar_version_t *packet = (mavlink_ret_lidar_version_t *)msgbuf; + + mav_array_memcpy(packet->sys_soft_version, sys_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet->com_soft_version, com_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet->sys_hard_version, sys_hard_version, sizeof(uint8_t)*6); + mav_array_memcpy(packet->com_hard_version, com_hard_version, sizeof(uint8_t)*6); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RET_LIDAR_VERSION, (const char *)packet, MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN, MAVLINK_MSG_ID_RET_LIDAR_VERSION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RET_LIDAR_VERSION UNPACKING + + +/** + * @brief Get field sys_soft_version from ret_lidar_version message + * + * @return SYS board soft version. + */ +static inline uint16_t mavlink_msg_ret_lidar_version_get_sys_soft_version(const mavlink_message_t* msg, uint8_t *sys_soft_version) +{ + return _MAV_RETURN_uint8_t_array(msg, sys_soft_version, 13, 0); +} + +/** + * @brief Get field com_soft_version from ret_lidar_version message + * + * @return COM board soft version. + */ +static inline uint16_t mavlink_msg_ret_lidar_version_get_com_soft_version(const mavlink_message_t* msg, uint8_t *com_soft_version) +{ + return _MAV_RETURN_uint8_t_array(msg, com_soft_version, 13, 13); +} + +/** + * @brief Get field sys_hard_version from ret_lidar_version message + * + * @return SYS board hard version. + */ +static inline uint16_t mavlink_msg_ret_lidar_version_get_sys_hard_version(const mavlink_message_t* msg, uint8_t *sys_hard_version) +{ + return _MAV_RETURN_uint8_t_array(msg, sys_hard_version, 6, 26); +} + +/** + * @brief Get field com_hard_version from ret_lidar_version message + * + * @return COM board hard version. + */ +static inline uint16_t mavlink_msg_ret_lidar_version_get_com_hard_version(const mavlink_message_t* msg, uint8_t *com_hard_version) +{ + return _MAV_RETURN_uint8_t_array(msg, com_hard_version, 6, 32); +} + +/** + * @brief Decode a ret_lidar_version message into a struct + * + * @param msg The message to decode + * @param ret_lidar_version C-struct to decode the message contents into + */ +static inline void mavlink_msg_ret_lidar_version_decode(const mavlink_message_t* msg, mavlink_ret_lidar_version_t* ret_lidar_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ret_lidar_version_get_sys_soft_version(msg, ret_lidar_version->sys_soft_version); + mavlink_msg_ret_lidar_version_get_com_soft_version(msg, ret_lidar_version->com_soft_version); + mavlink_msg_ret_lidar_version_get_sys_hard_version(msg, ret_lidar_version->sys_hard_version); + mavlink_msg_ret_lidar_version_get_com_hard_version(msg, ret_lidar_version->com_hard_version); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN? msg->len : MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN; + memset(ret_lidar_version, 0, MAVLINK_MSG_ID_RET_LIDAR_VERSION_LEN); + memcpy(ret_lidar_version, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/unitree_lidar_sdk/mavlink/SysMavlink/testsuite.h b/unitree_lidar_sdk/mavlink/SysMavlink/testsuite.h new file mode 100644 index 0000000..ea6d970 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/SysMavlink/testsuite.h @@ -0,0 +1,1026 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from SysMavlink.xml + * @see https://mavlink.io/en/ + */ +#pragma once +#ifndef SYSMAVLINK_TESTSUITE_H +#define SYSMAVLINK_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_SysMavlink(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_SysMavlink(system_id, component_id, last_msg); +} +#endif + + + + +static void mavlink_test_device_request_answer(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_DEVICE_REQUEST_ANSWER >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_device_request_answer_t packet_in = { + 5 + }; + mavlink_device_request_answer_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.command_return_ok = packet_in.command_return_ok; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEVICE_REQUEST_ANSWER_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEVICE_REQUEST_ANSWER_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_answer_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_device_request_answer_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_answer_pack(system_id, component_id, &msg , packet1.command_return_ok ); + mavlink_msg_device_request_answer_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_answer_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command_return_ok ); + mavlink_msg_device_request_answer_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_DEVICE_DEBUG_MODE_PARAMS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_device_debug_mode_params_t packet_in = { + 17.0,45.0,17651,17755,17859,17963,53,120,187,254 + }; + mavlink_device_debug_mode_params_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.debug_param_value = packet_in.debug_param_value; + packet1.debug_float_reserve = packet_in.debug_float_reserve; + packet1.debug_pos = packet_in.debug_pos; + packet1.debug_count = packet_in.debug_count; + packet1.debug_laser_cycle = packet_in.debug_laser_cycle; + packet1.debug_uin16_t_reserve = packet_in.debug_uin16_t_reserve; + packet1.debug_mode = packet_in.debug_mode; + packet1.debug_needed = packet_in.debug_needed; + packet1.debug_first_test_flag = packet_in.debug_first_test_flag; + packet1.debug_uin8_t_reserve = packet_in.debug_uin8_t_reserve; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEVICE_DEBUG_MODE_PARAMS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEVICE_DEBUG_MODE_PARAMS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_debug_mode_params_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_device_debug_mode_params_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_debug_mode_params_pack(system_id, component_id, &msg , packet1.debug_mode , packet1.debug_needed , packet1.debug_first_test_flag , packet1.debug_uin8_t_reserve , packet1.debug_pos , packet1.debug_count , packet1.debug_laser_cycle , packet1.debug_uin16_t_reserve , packet1.debug_param_value , packet1.debug_float_reserve ); + mavlink_msg_device_debug_mode_params_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_debug_mode_params_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.debug_mode , packet1.debug_needed , packet1.debug_first_test_flag , packet1.debug_uin8_t_reserve , packet1.debug_pos , packet1.debug_count , packet1.debug_laser_cycle , packet1.debug_uin16_t_reserve , packet1.debug_param_value , packet1.debug_float_reserve ); + mavlink_msg_device_debug_mode_params_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SET_COM_NECESSARY_MEASURE_PARAMETER >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_com_necessary_measure_parameter_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,963498920,963499128,963499336,963499544,963499752,963499960,19939,20043,20147,20251,185 + }; + mavlink_set_com_necessary_measure_parameter_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.moto_target_speed = packet_in.moto_target_speed; + packet1.moto_control_coef = packet_in.moto_control_coef; + packet1.moto_speed_threshold = packet_in.moto_speed_threshold; + packet1.imu_target_tempe = packet_in.imu_target_tempe; + packet1.imu_x_axis_error = packet_in.imu_x_axis_error; + packet1.imu_y_axis_error = packet_in.imu_y_axis_error; + packet1.imu_z_axis_error = packet_in.imu_z_axis_error; + packet1.imu_acc_bias_x = packet_in.imu_acc_bias_x; + packet1.imu_acc_bias_y = packet_in.imu_acc_bias_y; + packet1.imu_acc_bias_z = packet_in.imu_acc_bias_z; + packet1.imu_gyro_bias_x = packet_in.imu_gyro_bias_x; + packet1.imu_gyro_bias_y = packet_in.imu_gyro_bias_y; + packet1.imu_gyro_bias_z = packet_in.imu_gyro_bias_z; + packet1.flash_ok_flag = packet_in.flash_ok_flag; + packet1.led_mode_config = packet_in.led_mode_config; + packet1.imu_work_mode_select = packet_in.imu_work_mode_select; + packet1.moto_pwm_duty = packet_in.moto_pwm_duty; + packet1.factory_mode_en = packet_in.factory_mode_en; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SET_COM_NECESSARY_MEASURE_PARAMETER_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SET_COM_NECESSARY_MEASURE_PARAMETER_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_com_necessary_measure_parameter_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_com_necessary_measure_parameter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_com_necessary_measure_parameter_pack(system_id, component_id, &msg , packet1.factory_mode_en , packet1.flash_ok_flag , packet1.led_mode_config , packet1.imu_work_mode_select , packet1.moto_pwm_duty , packet1.moto_target_speed , packet1.moto_control_coef , packet1.moto_speed_threshold , packet1.imu_target_tempe , packet1.imu_x_axis_error , packet1.imu_y_axis_error , packet1.imu_z_axis_error , packet1.imu_acc_bias_x , packet1.imu_acc_bias_y , packet1.imu_acc_bias_z , packet1.imu_gyro_bias_x , packet1.imu_gyro_bias_y , packet1.imu_gyro_bias_z ); + mavlink_msg_set_com_necessary_measure_parameter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_com_necessary_measure_parameter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.factory_mode_en , packet1.flash_ok_flag , packet1.led_mode_config , packet1.imu_work_mode_select , packet1.moto_pwm_duty , packet1.moto_target_speed , packet1.moto_control_coef , packet1.moto_speed_threshold , packet1.imu_target_tempe , packet1.imu_x_axis_error , packet1.imu_y_axis_error , packet1.imu_z_axis_error , packet1.imu_acc_bias_x , packet1.imu_acc_bias_y , packet1.imu_acc_bias_z , packet1.imu_gyro_bias_x , packet1.imu_gyro_bias_y , packet1.imu_gyro_bias_z ); + mavlink_msg_set_com_necessary_measure_parameter_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SET_SYS_NECESSARY_MEASURE_PARAMETER >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_sys_necessary_measure_parameter_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,19107,19211,19315,3,70 + }; + mavlink_set_sys_necessary_measure_parameter_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.tdc_bin_length = packet_in.tdc_bin_length; + packet1.apd_temp_param = packet_in.apd_temp_param; + packet1.gray_param_a = packet_in.gray_param_a; + packet1.gray_param_b = packet_in.gray_param_b; + packet1.gray_param_c = packet_in.gray_param_c; + packet1.reflect_param = packet_in.reflect_param; + packet1.b_axis_dist = packet_in.b_axis_dist; + packet1.theta_angle = packet_in.theta_angle; + packet1.ksi_angle = packet_in.ksi_angle; + packet1.comparator_voltage = packet_in.comparator_voltage; + packet1.zero_offset_bin = packet_in.zero_offset_bin; + packet1.flash_ok_flag = packet_in.flash_ok_flag; + packet1.start_offset = packet_in.start_offset; + packet1.gray_calibration_en = packet_in.gray_calibration_en; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SET_SYS_NECESSARY_MEASURE_PARAMETER_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SET_SYS_NECESSARY_MEASURE_PARAMETER_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_sys_necessary_measure_parameter_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_sys_necessary_measure_parameter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_sys_necessary_measure_parameter_pack(system_id, component_id, &msg , packet1.start_offset , packet1.gray_calibration_en , packet1.comparator_voltage , packet1.zero_offset_bin , packet1.flash_ok_flag , packet1.tdc_bin_length , packet1.apd_temp_param , packet1.gray_param_a , packet1.gray_param_b , packet1.gray_param_c , packet1.reflect_param , packet1.b_axis_dist , packet1.theta_angle , packet1.ksi_angle ); + mavlink_msg_set_sys_necessary_measure_parameter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_sys_necessary_measure_parameter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.start_offset , packet1.gray_calibration_en , packet1.comparator_voltage , packet1.zero_offset_bin , packet1.flash_ok_flag , packet1.tdc_bin_length , packet1.apd_temp_param , packet1.gray_param_a , packet1.gray_param_b , packet1.gray_param_c , packet1.reflect_param , packet1.b_axis_dist , packet1.theta_angle , packet1.ksi_angle ); + mavlink_msg_set_sys_necessary_measure_parameter_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RET_TDC_POINT_DEBUG_PACKET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ret_tdc_point_debug_packet_t packet_in = { + 17.0,45.0,73.0,101.0,18067,18171,18275,18379,18483,18587,18691,18795,18899,235,46 + }; + mavlink_ret_tdc_point_debug_packet_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.width_correction_diff = packet_in.width_correction_diff; + packet1.apd_temperature = packet_in.apd_temperature; + packet1.imu_temperature = packet_in.imu_temperature; + packet1.apd_voltage = packet_in.apd_voltage; + packet1.start = packet_in.start; + packet1.width = packet_in.width; + packet1.spec_start = packet_in.spec_start; + packet1.spec_width = packet_in.spec_width; + packet1.dist = packet_in.dist; + packet1.point_index = packet_in.point_index; + packet1.reserve_2 = packet_in.reserve_2; + packet1.reserve_3 = packet_in.reserve_3; + packet1.reserve_4 = packet_in.reserve_4; + packet1.spec_reflectivity = packet_in.spec_reflectivity; + packet1.packet_index = packet_in.packet_index; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RET_TDC_POINT_DEBUG_PACKET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RET_TDC_POINT_DEBUG_PACKET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_tdc_point_debug_packet_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ret_tdc_point_debug_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_tdc_point_debug_packet_pack(system_id, component_id, &msg , packet1.spec_reflectivity , packet1.packet_index , packet1.start , packet1.width , packet1.spec_start , packet1.spec_width , packet1.dist , packet1.point_index , packet1.reserve_2 , packet1.reserve_3 , packet1.reserve_4 , packet1.width_correction_diff , packet1.apd_temperature , packet1.imu_temperature , packet1.apd_voltage ); + mavlink_msg_ret_tdc_point_debug_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_tdc_point_debug_packet_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.spec_reflectivity , packet1.packet_index , packet1.start , packet1.width , packet1.spec_start , packet1.spec_width , packet1.dist , packet1.point_index , packet1.reserve_2 , packet1.reserve_3 , packet1.reserve_4 , packet1.width_correction_diff , packet1.apd_temperature , packet1.imu_temperature , packet1.apd_voltage ); + mavlink_msg_ret_tdc_point_debug_packet_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_DEVICE_REQUEST_INTERNAL_DATA >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_device_request_internal_data_t packet_in = { + 5 + }; + mavlink_device_request_internal_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.request_type = packet_in.request_type; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEVICE_REQUEST_INTERNAL_DATA_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEVICE_REQUEST_INTERNAL_DATA_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_internal_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_device_request_internal_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_internal_data_pack(system_id, component_id, &msg , packet1.request_type ); + mavlink_msg_device_request_internal_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_internal_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.request_type ); + mavlink_msg_device_request_internal_data_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_DEVICE_REQUEST_DATA >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_device_request_data_t packet_in = { + 5 + }; + mavlink_device_request_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.request_type = packet_in.request_type; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEVICE_REQUEST_DATA_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_device_request_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_data_pack(system_id, component_id, &msg , packet1.request_type ); + mavlink_msg_device_request_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_request_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.request_type ); + mavlink_msg_device_request_data_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_DEVICE_COMMAND >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_device_command_t packet_in = { + 5 + }; + mavlink_device_command_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.cmd_type = packet_in.cmd_type; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEVICE_COMMAND_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_command_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_device_command_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_command_pack(system_id, component_id, &msg , packet1.cmd_type ); + mavlink_msg_device_command_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_device_command_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cmd_type ); + mavlink_msg_device_command_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RET_LIDAR_VERSION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ret_lidar_version_t packet_in = { + { 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17 },{ 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120 },{ 211, 212, 213, 214, 215, 216 },{ 101, 102, 103, 104, 105, 106 } + }; + mavlink_ret_lidar_version_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + + mav_array_memcpy(packet1.sys_soft_version, packet_in.sys_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet1.com_soft_version, packet_in.com_soft_version, sizeof(uint8_t)*13); + mav_array_memcpy(packet1.sys_hard_version, packet_in.sys_hard_version, sizeof(uint8_t)*6); + mav_array_memcpy(packet1.com_hard_version, packet_in.com_hard_version, sizeof(uint8_t)*6); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RET_LIDAR_VERSION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_version_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ret_lidar_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_version_pack(system_id, component_id, &msg , packet1.sys_soft_version , packet1.com_soft_version , packet1.sys_hard_version , packet1.com_hard_version ); + mavlink_msg_ret_lidar_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sys_soft_version , packet1.com_soft_version , packet1.sys_hard_version , packet1.com_hard_version ); + mavlink_msg_ret_lidar_version_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_config_lidar_working_mode_t packet_in = { + 5 + }; + mavlink_config_lidar_working_mode_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.request_type = packet_in.request_type; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CONFIG_LIDAR_WORKING_MODE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_config_lidar_working_mode_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_config_lidar_working_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_config_lidar_working_mode_pack(system_id, component_id, &msg , packet1.request_type ); + mavlink_msg_config_lidar_working_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_config_lidar_working_mode_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.request_type ); + mavlink_msg_config_lidar_working_mode_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_config_led_ring_table_packet_t packet_in = { + 963497464,963497672,17651,163,{ 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18 } + }; + mavlink_config_led_ring_table_packet_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.led_rotation_period = packet_in.led_rotation_period; + packet1.led_zero_point_offset = packet_in.led_zero_point_offset; + packet1.packet_id = packet_in.packet_id; + packet1.led_display_mode = packet_in.led_display_mode; + + mav_array_memcpy(packet1.led_table, packet_in.led_table, sizeof(uint8_t)*45); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CONFIG_LED_RING_TABLE_PACKET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_config_led_ring_table_packet_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_config_led_ring_table_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_config_led_ring_table_packet_pack(system_id, component_id, &msg , packet1.packet_id , packet1.led_rotation_period , packet1.led_zero_point_offset , packet1.led_display_mode , packet1.led_table ); + mavlink_msg_config_led_ring_table_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_config_led_ring_table_packet_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.packet_id , packet1.led_rotation_period , packet1.led_zero_point_offset , packet1.led_display_mode , packet1.led_table ); + mavlink_msg_config_led_ring_table_packet_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ret_lidar_distance_data_packet_t packet_in = { + 17235,17339,17443,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134 } + }; + mavlink_ret_lidar_distance_data_packet_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.packet_id = packet_in.packet_id; + packet1.packet_cnt = packet_in.packet_cnt; + packet1.payload_size = packet_in.payload_size; + + mav_array_memcpy(packet1.point_data, packet_in.point_data, sizeof(uint8_t)*240); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RET_LIDAR_DISTANCE_DATA_PACKET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_distance_data_packet_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ret_lidar_distance_data_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_distance_data_packet_pack(system_id, component_id, &msg , packet1.packet_id , packet1.packet_cnt , packet1.payload_size , packet1.point_data ); + mavlink_msg_ret_lidar_distance_data_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_distance_data_packet_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.packet_id , packet1.packet_cnt , packet1.payload_size , packet1.point_data ); + mavlink_msg_ret_lidar_distance_data_packet_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ret_lidar_auxiliary_data_packet_t packet_in = { + 963497464,963497672,963497880,963498088,963498296,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,465.0,493.0,521.0,549.0,577.0,21603,21707,13,{ 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199 } + }; + mavlink_ret_lidar_auxiliary_data_packet_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.lidar_sync_delay_time = packet_in.lidar_sync_delay_time; + packet1.time_stamp_s_step = packet_in.time_stamp_s_step; + packet1.time_stamp_us_step = packet_in.time_stamp_us_step; + packet1.sys_rotation_period = packet_in.sys_rotation_period; + packet1.com_rotation_period = packet_in.com_rotation_period; + packet1.com_horizontal_angle_start = packet_in.com_horizontal_angle_start; + packet1.com_horizontal_angle_step = packet_in.com_horizontal_angle_step; + packet1.sys_vertical_angle_start = packet_in.sys_vertical_angle_start; + packet1.sys_vertical_angle_span = packet_in.sys_vertical_angle_span; + packet1.apd_temperature = packet_in.apd_temperature; + packet1.dirty_index = packet_in.dirty_index; + packet1.imu_temperature = packet_in.imu_temperature; + packet1.up_optical_q = packet_in.up_optical_q; + packet1.down_optical_q = packet_in.down_optical_q; + packet1.apd_voltage = packet_in.apd_voltage; + packet1.imu_angle_x_offset = packet_in.imu_angle_x_offset; + packet1.imu_angle_y_offset = packet_in.imu_angle_y_offset; + packet1.imu_angle_z_offset = packet_in.imu_angle_z_offset; + packet1.b_axis_dist = packet_in.b_axis_dist; + packet1.theta_angle = packet_in.theta_angle; + packet1.ksi_angle = packet_in.ksi_angle; + packet1.packet_id = packet_in.packet_id; + packet1.payload_size = packet_in.payload_size; + packet1.lidar_work_status = packet_in.lidar_work_status; + + mav_array_memcpy(packet1.reflect_data, packet_in.reflect_data, sizeof(uint8_t)*120); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RET_LIDAR_AUXILIARY_DATA_PACKET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_auxiliary_data_packet_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ret_lidar_auxiliary_data_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_auxiliary_data_packet_pack(system_id, component_id, &msg , packet1.lidar_work_status , packet1.packet_id , packet1.payload_size , packet1.lidar_sync_delay_time , packet1.time_stamp_s_step , packet1.time_stamp_us_step , packet1.sys_rotation_period , packet1.com_rotation_period , packet1.com_horizontal_angle_start , packet1.com_horizontal_angle_step , packet1.sys_vertical_angle_start , packet1.sys_vertical_angle_span , packet1.apd_temperature , packet1.dirty_index , packet1.imu_temperature , packet1.up_optical_q , packet1.down_optical_q , packet1.apd_voltage , packet1.imu_angle_x_offset , packet1.imu_angle_y_offset , packet1.imu_angle_z_offset , packet1.b_axis_dist , packet1.theta_angle , packet1.ksi_angle , packet1.reflect_data ); + mavlink_msg_ret_lidar_auxiliary_data_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_auxiliary_data_packet_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lidar_work_status , packet1.packet_id , packet1.payload_size , packet1.lidar_sync_delay_time , packet1.time_stamp_s_step , packet1.time_stamp_us_step , packet1.sys_rotation_period , packet1.com_rotation_period , packet1.com_horizontal_angle_start , packet1.com_horizontal_angle_step , packet1.sys_vertical_angle_start , packet1.sys_vertical_angle_span , packet1.apd_temperature , packet1.dirty_index , packet1.imu_temperature , packet1.up_optical_q , packet1.down_optical_q , packet1.apd_voltage , packet1.imu_angle_x_offset , packet1.imu_angle_y_offset , packet1.imu_angle_z_offset , packet1.b_axis_dist , packet1.theta_angle , packet1.ksi_angle , packet1.reflect_data ); + mavlink_msg_ret_lidar_auxiliary_data_packet_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ret_lidar_time_sync_data_t packet_in = { + 963497464,17 + }; + mavlink_ret_lidar_time_sync_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.lidar_sync_count = packet_in.lidar_sync_count; + packet1.lidar_sync_type = packet_in.lidar_sync_type; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RET_LIDAR_TIME_SYNC_DATA_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_time_sync_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ret_lidar_time_sync_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_time_sync_data_pack(system_id, component_id, &msg , packet1.lidar_sync_type , packet1.lidar_sync_count ); + mavlink_msg_ret_lidar_time_sync_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_lidar_time_sync_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lidar_sync_type , packet1.lidar_sync_count ); + mavlink_msg_ret_lidar_time_sync_data_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ret_imu_attitude_data_packet_t packet_in = { + { 17.0, 18.0, 19.0, 20.0 },{ 129.0, 130.0, 131.0 },{ 213.0, 214.0, 215.0 },19315 + }; + mavlink_ret_imu_attitude_data_packet_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.packet_id = packet_in.packet_id; + + mav_array_memcpy(packet1.quaternion, packet_in.quaternion, sizeof(float)*4); + mav_array_memcpy(packet1.angular_velocity, packet_in.angular_velocity, sizeof(float)*3); + mav_array_memcpy(packet1.linear_acceleration, packet_in.linear_acceleration, sizeof(float)*3); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RET_IMU_ATTITUDE_DATA_PACKET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_imu_attitude_data_packet_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ret_imu_attitude_data_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_imu_attitude_data_packet_pack(system_id, component_id, &msg , packet1.packet_id , packet1.quaternion , packet1.angular_velocity , packet1.linear_acceleration ); + mavlink_msg_ret_imu_attitude_data_packet_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ret_imu_attitude_data_packet_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.packet_id , packet1.quaternion , packet1.angular_velocity , packet1.linear_acceleration ); + mavlink_msg_ret_imu_attitude_data_packet_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 + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +#ifndef HAVE_CRC_ACCUMULATE +/** + * @brief Accumulate the CRC16_MCRF4XX checksum 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 Initialize the buffer for the MCRF4XX CRC16 + * + * @param crcAccum the 16 bit MCRF4XX CRC16 + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + + +/** + * @brief Calculates the CRC16_MCRF4XX 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 MCRF4XX CRC16 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/unitree_lidar_sdk/mavlink/mavlink_conversions.h b/unitree_lidar_sdk/mavlink/mavlink_conversions.h new file mode 100644 index 0000000..c788023 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/mavlink_conversions.h @@ -0,0 +1,212 @@ +#pragma once + +#ifndef MAVLINK_NO_CONVERSION_HELPERS + +/* enable math defines on Windows */ +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif +#include + +#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 + */ + + +/** + * 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 = (double)quaternion[0]; + double b = (double)quaternion[1]; + double c = (double)quaternion[2]; + double d = (double)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 = asinf(-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; +} + +#endif // MAVLINK_NO_CONVERSION_HELPERS diff --git a/unitree_lidar_sdk/mavlink/mavlink_get_info.h b/unitree_lidar_sdk/mavlink/mavlink_get_info.h new file mode 100644 index 0000000..5f5130b --- /dev/null +++ b/unitree_lidar_sdk/mavlink/mavlink_get_info.h @@ -0,0 +1,83 @@ +#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_by_id(uint32_t msgid) +{ + 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 + */ + const uint32_t count = sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]); + if (count == 0) { + return NULL; + } + uint32_t low=0, high=count-1; + while (low < high) { + uint32_t mid = (low+high)/2; + if (msgid < mavlink_message_info[mid].msgid) { + high = mid; + continue; + } + if (msgid > mavlink_message_info[mid].msgid) { + low = mid+1; + continue; + } + return &mavlink_message_info[mid]; + } + if (mavlink_message_info[low].msgid == msgid) { + return &mavlink_message_info[low]; + } + return NULL; +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) +{ + return mavlink_get_message_info_by_id(msg->msgid); +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) +{ + static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; + /* + 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 name + */ + const uint32_t count = sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); + if (count == 0) { + return NULL; + } + uint32_t low=0, high=count-1; + while (low < high) { + uint32_t mid = (low+high)/2; + int cmp = strcmp(mavlink_message_names[mid].name, name); + if (cmp > 0) { + high = mid; + continue; + } + if (cmp < 0) { + low = mid+1; + continue; + } + low = mid; + break; + } + if (strcmp(mavlink_message_names[low].name, name) == 0) { + return mavlink_get_message_info_by_id(mavlink_message_names[low].msgid); + } + return NULL; +} +#endif // MAVLINK_USE_MESSAGE_INFO + + diff --git a/unitree_lidar_sdk/mavlink/mavlink_helpers.h b/unitree_lidar_sdk/mavlink/mavlink_helpers.h new file mode 100644 index 0000000..45e8ecc --- /dev/null +++ b/unitree_lidar_sdk/mavlink/mavlink_helpers.h @@ -0,0 +1,1134 @@ +#pragma once + +#include "string.h" +#include "checksum.h" +#include "mavlink_types.h" +#include "mavlink_conversions.h" +#include + +#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 + +/* 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. +*/ +//#define MAVLINK_CHECK_MESSAGE_LENGTH + +/** + * @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; +} + +/** + * @brief Trim payload of any trailing zero-populated bytes (MAVLink 2 only). + * + * @param payload Serialised payload buffer. + * @param length Length of full-width payload buffer. + * @return Length of payload after zero-filled bytes are trimmed. + */ +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_NUM_HEADER_BYTES); + mavlink_sha256_update(&ctx, _MAV_PAYLOAD(msg), 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; inum_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; + } + + uint16_t checksum = crc_calculate(&buf[1], header_len-1); + crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len); + crc_accumulate(crc_extra, &checksum); + mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + + msg->checksum = checksum; + + 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) +{ + uint16_t crcTmp = 0; + crc_init(&crcTmp); + msg->checksum = crcTmp; +} + +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + uint16_t checksum = msg->checksum; + crc_accumulate(c, &checksum); + msg->checksum = 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]) - 1; + 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 min message length +*/ +#define MAVLINK_HAVE_MIN_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_min_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->min_msg_len:0; +} + +/* + return the max message length (including extensions) +*/ +#define MAVLINK_HAVE_MAX_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_max_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->max_msg_len:0; +} + +/** + * This is a variant 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 status buffer + * @param c The char to parse + * + * @param r_message NULL if no message could be decoded, otherwise the message data + * @param r_mavlink_status 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 + * + */ +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) +{ + 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) { + if(rxmsg->len > 0) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) { + _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 |= ((uint32_t)c)<<16; + mavlink_update_checksum(rxmsg, c); + if(rxmsg->len > 0){ + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) + { + _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->max_msg_len) { + memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_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; + if (r_message != NULL) { + 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; + if (r_message !=NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } + break; + } + + bufferIndex++; + // If a message has been successfully 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++; + } + + if (r_message != NULL) { + r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg + } + if (r_mavlink_status != NULL) { + 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 + */ + if (r_message != NULL) { + 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 *r_message and the channel's status is + * copied into *r_mavlink_status. + * + * @param chan ID of the channel to be parsed. + * A channel is not a physical message channel like a serial port, but a logical partition of + * the communication streams. 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 r_message NULL if no message could be decoded, otherwise the message data + * @param r_mavlink_status 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_status_t status; + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_frame_char(chan, byte, &msg, &status) != 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 *r_message and the channel's status is + * copied into *r_mavlink_status. + * + * @param chan ID of the channel to be parsed. + * A channel is not a physical message channel like a serial port, but a logical partition of + * the communication streams. 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 r_message NULL if no message could be decoded, otherwise the message data + * @param r_mavlink_status 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_status_t status; + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg, &status)) + * { + * 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/unitree_lidar_sdk/mavlink/mavlink_sha256.h b/unitree_lidar_sdk/mavlink/mavlink_sha256.h new file mode 100644 index 0000000..a45410e --- /dev/null +++ b/unitree_lidar_sdk/mavlink/mavlink_sha256.h @@ -0,0 +1,235 @@ +#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 { + uint32_t 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)) + +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; + m->counter[0] = 0x6a09e667; + m->counter[1] = 0xbb67ae85; + m->counter[2] = 0x3c6ef372; + m->counter[3] = 0xa54ff53a; + m->counter[4] = 0x510e527f; + m->counter[5] = 0x9b05688c; + m->counter[6] = 0x1f83d9ab; + m->counter[7] = 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 = m->counter[0]; + BB = m->counter[1]; + CC = m->counter[2]; + DD = m->counter[3]; + EE = m->counter[4]; + FF = m->counter[5]; + GG = m->counter[6]; + HH = m->counter[7]; + + 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; + } + + m->counter[0] += AA; + m->counter[1] += BB; + m->counter[2] += CC; + m->counter[3] += DD; + m->counter[4] += EE; + m->counter[5] += FF; + m->counter[6] += GG; + m->counter[7] += 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 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/unitree_lidar_sdk/mavlink/mavlink_types.h b/unitree_lidar_sdk/mavlink/mavlink_types.h new file mode 100644 index 0000000..2dcd834 --- /dev/null +++ b/unitree_lidar_sdk/mavlink/mavlink_types.h @@ -0,0 +1,301 @@ +#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 +#include + +#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)) + +#ifndef HAVE_MAVLINK_CHANNEL_T +typedef enum { + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3 +} mavlink_channel_t; +#endif + +/* + * 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 ///< Enable outgoing signing + +/* + state of MAVLink signing for this channel + */ +typedef struct __mavlink_signing { + uint8_t flags; ///< MAVLINK_SIGNING_FLAG_* + uint8_t link_id; ///< Same as MAVLINK_CHANNEL + uint64_t timestamp; ///< Timestamp, in microseconds since UNIX epoch GMT + 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; ///< ID of the link (MAVLINK_CHANNEL) + uint8_t sysid; ///< Remote system ID + uint8_t compid; ///< Remote component ID + uint8_t timestamp_bytes[6]; ///< Timestamp, in microseconds since UNIX epoch GMT + } 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 min_msg_len; // minimum message length + uint8_t max_msg_len; // maximum message length (e.g. including mavlink2 extensions) + 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/unitree_lidar_sdk/mavlink/protocol.h b/unitree_lidar_sdk/mavlink/protocol.h new file mode 100644 index 0000000..22bd0ca --- /dev/null +++ b/unitree_lidar_sdk/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