123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658 |
- #pragma once
- // MESSAGE VK_FORMATION_LEADER PACKING
- #define MAVLINK_MSG_ID_VK_FORMATION_LEADER 53004
- MAVPACKED(
- typedef struct __mavlink_vk_formation_leader_t {
- uint32_t timestamp; /*< [ms] Timestamp in ms from system boot.*/
- uint32_t state; /*< formation leader drone state bitmap*/
- int32_t lat; /*< [degE7] formation leader latitude in 1e-7deg */
- int32_t lon; /*< [degE7] formation leader longitude in 1e-7deg*/
- float msl; /*< [m] formation leader msl altitude in meter*/
- float ve; /*< [m/s] formation leader east speed*/
- float vn; /*< [m/s] formation leader north speed*/
- float vu; /*< [m/s] formation leader up speed*/
- float yaw; /*< [deg] formation leader yaw*/
- int16_t x_dist; /*< [cm] distance between drones in x axis*/
- int16_t y_dist; /*< [cm] distance between drones in y axis*/
- int16_t z_dist; /*< [cm] distance between drones in z axis*/
- uint16_t rect_col_num; /*< columns number of rectangle formation*/
- uint8_t formation_type; /*< formation type*/
- float formation_heading; /*< [deg] if nan, use yaw as formation
- heading*/
- }) mavlink_vk_formation_leader_t;
- #define MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN 49
- #define MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN 45
- #define MAVLINK_MSG_ID_53004_LEN 49
- #define MAVLINK_MSG_ID_53004_MIN_LEN 45
- #define MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC 219
- #define MAVLINK_MSG_ID_53004_CRC 219
- #if MAVLINK_COMMAND_24BIT
- #define MAVLINK_MESSAGE_INFO_VK_FORMATION_LEADER { \
- 53004, \
- "VK_FORMATION_LEADER", \
- 15, \
- { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_vk_formation_leader_t, timestamp) }, \
- { "state", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_vk_formation_leader_t, state) }, \
- { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_vk_formation_leader_t, lat) }, \
- { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_vk_formation_leader_t, lon) }, \
- { "msl", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vk_formation_leader_t, msl) }, \
- { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vk_formation_leader_t, ve) }, \
- { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vk_formation_leader_t, vn) }, \
- { "vu", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vk_formation_leader_t, vu) }, \
- { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_vk_formation_leader_t, yaw) }, \
- { "formation_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_vk_formation_leader_t, formation_type) }, \
- { "x_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_vk_formation_leader_t, x_dist) }, \
- { "y_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_vk_formation_leader_t, y_dist) }, \
- { "z_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_vk_formation_leader_t, z_dist) }, \
- { "rect_col_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_vk_formation_leader_t, rect_col_num) }, \
- { "formation_heading", NULL, MAVLINK_TYPE_FLOAT, 0, 45, offsetof(mavlink_vk_formation_leader_t, formation_heading) }, \
- } \
- }
- #else
- #define MAVLINK_MESSAGE_INFO_VK_FORMATION_LEADER { \
- "VK_FORMATION_LEADER", \
- 15, \
- { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_vk_formation_leader_t, timestamp) }, \
- { "state", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_vk_formation_leader_t, state) }, \
- { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_vk_formation_leader_t, lat) }, \
- { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_vk_formation_leader_t, lon) }, \
- { "msl", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vk_formation_leader_t, msl) }, \
- { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vk_formation_leader_t, ve) }, \
- { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vk_formation_leader_t, vn) }, \
- { "vu", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vk_formation_leader_t, vu) }, \
- { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_vk_formation_leader_t, yaw) }, \
- { "formation_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_vk_formation_leader_t, formation_type) }, \
- { "x_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_vk_formation_leader_t, x_dist) }, \
- { "y_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_vk_formation_leader_t, y_dist) }, \
- { "z_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_vk_formation_leader_t, z_dist) }, \
- { "rect_col_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_vk_formation_leader_t, rect_col_num) }, \
- { "formation_heading", NULL, MAVLINK_TYPE_FLOAT, 0, 45, offsetof(mavlink_vk_formation_leader_t, formation_heading) }, \
- } \
- }
- #endif
- /**
- * @brief Pack a vk_formation_leader message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param timestamp [ms] Timestamp in ms from system boot.
- * @param state formation leader drone state bitmap
- * @param lat [degE7] formation leader latitude in 1e-7deg
- * @param lon [degE7] formation leader longitude in 1e-7deg
- * @param msl [m] formation leader msl altitude in meter
- * @param ve [m/s] formation leader east speed
- * @param vn [m/s] formation leader north speed
- * @param vu [m/s] formation leader up speed
- * @param yaw [deg] formation leader yaw
- * @param formation_type formation type
- * @param x_dist [cm] distance between drones in x axis
- * @param y_dist [cm] distance between drones in y axis
- * @param z_dist [cm] distance between drones in z axis
- * @param rect_col_num columns number of rectangle formation
- * @param formation_heading [deg] if nan, use yaw as formation
- heading
- * @return length of the message in bytes (excluding serial stream start sign)
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
- uint32_t timestamp, uint32_t state, int32_t lat, int32_t lon, float msl, float ve, float vn, float vu, float yaw, uint8_t formation_type, int16_t x_dist, int16_t y_dist, int16_t z_dist, uint16_t rect_col_num, float formation_heading)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
- _mav_put_uint32_t(buf, 0, timestamp);
- _mav_put_uint32_t(buf, 4, state);
- _mav_put_int32_t(buf, 8, lat);
- _mav_put_int32_t(buf, 12, lon);
- _mav_put_float(buf, 16, msl);
- _mav_put_float(buf, 20, ve);
- _mav_put_float(buf, 24, vn);
- _mav_put_float(buf, 28, vu);
- _mav_put_float(buf, 32, yaw);
- _mav_put_int16_t(buf, 36, x_dist);
- _mav_put_int16_t(buf, 38, y_dist);
- _mav_put_int16_t(buf, 40, z_dist);
- _mav_put_uint16_t(buf, 42, rect_col_num);
- _mav_put_uint8_t(buf, 44, formation_type);
- _mav_put_float(buf, 45, formation_heading);
- memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #else
- mavlink_vk_formation_leader_t packet;
- packet.timestamp = timestamp;
- packet.state = state;
- packet.lat = lat;
- packet.lon = lon;
- packet.msl = msl;
- packet.ve = ve;
- packet.vn = vn;
- packet.vu = vu;
- packet.yaw = yaw;
- packet.x_dist = x_dist;
- packet.y_dist = y_dist;
- packet.z_dist = z_dist;
- packet.rect_col_num = rect_col_num;
- packet.formation_type = formation_type;
- packet.formation_heading = formation_heading;
- memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #endif
- msg->msgid = MAVLINK_MSG_ID_VK_FORMATION_LEADER;
- return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- }
- /**
- * @brief Pack a vk_formation_leader message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param status MAVLink status structure
- * @param msg The MAVLink message to compress the data into
- *
- * @param timestamp [ms] Timestamp in ms from system boot.
- * @param state formation leader drone state bitmap
- * @param lat [degE7] formation leader latitude in 1e-7deg
- * @param lon [degE7] formation leader longitude in 1e-7deg
- * @param msl [m] formation leader msl altitude in meter
- * @param ve [m/s] formation leader east speed
- * @param vn [m/s] formation leader north speed
- * @param vu [m/s] formation leader up speed
- * @param yaw [deg] formation leader yaw
- * @param formation_type formation type
- * @param x_dist [cm] distance between drones in x axis
- * @param y_dist [cm] distance between drones in y axis
- * @param z_dist [cm] distance between drones in z axis
- * @param rect_col_num columns number of rectangle formation
- * @param formation_heading [deg] if nan, use yaw as formation
- heading
- * @return length of the message in bytes (excluding serial stream start sign)
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
- uint32_t timestamp, uint32_t state, int32_t lat, int32_t lon, float msl, float ve, float vn, float vu, float yaw, uint8_t formation_type, int16_t x_dist, int16_t y_dist, int16_t z_dist, uint16_t rect_col_num, float formation_heading)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
- _mav_put_uint32_t(buf, 0, timestamp);
- _mav_put_uint32_t(buf, 4, state);
- _mav_put_int32_t(buf, 8, lat);
- _mav_put_int32_t(buf, 12, lon);
- _mav_put_float(buf, 16, msl);
- _mav_put_float(buf, 20, ve);
- _mav_put_float(buf, 24, vn);
- _mav_put_float(buf, 28, vu);
- _mav_put_float(buf, 32, yaw);
- _mav_put_int16_t(buf, 36, x_dist);
- _mav_put_int16_t(buf, 38, y_dist);
- _mav_put_int16_t(buf, 40, z_dist);
- _mav_put_uint16_t(buf, 42, rect_col_num);
- _mav_put_uint8_t(buf, 44, formation_type);
- _mav_put_float(buf, 45, formation_heading);
- memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #else
- mavlink_vk_formation_leader_t packet;
- packet.timestamp = timestamp;
- packet.state = state;
- packet.lat = lat;
- packet.lon = lon;
- packet.msl = msl;
- packet.ve = ve;
- packet.vn = vn;
- packet.vu = vu;
- packet.yaw = yaw;
- packet.x_dist = x_dist;
- packet.y_dist = y_dist;
- packet.z_dist = z_dist;
- packet.rect_col_num = rect_col_num;
- packet.formation_type = formation_type;
- packet.formation_heading = formation_heading;
- memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #endif
- msg->msgid = MAVLINK_MSG_ID_VK_FORMATION_LEADER;
- #if MAVLINK_CRC_EXTRA
- return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- #else
- return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #endif
- }
- /**
- * @brief Pack a vk_formation_leader message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message will be sent over
- * @param msg The MAVLink message to compress the data into
- * @param timestamp [ms] Timestamp in ms from system boot.
- * @param state formation leader drone state bitmap
- * @param lat [degE7] formation leader latitude in 1e-7deg
- * @param lon [degE7] formation leader longitude in 1e-7deg
- * @param msl [m] formation leader msl altitude in meter
- * @param ve [m/s] formation leader east speed
- * @param vn [m/s] formation leader north speed
- * @param vu [m/s] formation leader up speed
- * @param yaw [deg] formation leader yaw
- * @param formation_type formation type
- * @param x_dist [cm] distance between drones in x axis
- * @param y_dist [cm] distance between drones in y axis
- * @param z_dist [cm] distance between drones in z axis
- * @param rect_col_num columns number of rectangle formation
- * @param formation_heading [deg] if nan, use yaw as formation
- heading
- * @return length of the message in bytes (excluding serial stream start sign)
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
- mavlink_message_t* msg,
- uint32_t timestamp,uint32_t state,int32_t lat,int32_t lon,float msl,float ve,float vn,float vu,float yaw,uint8_t formation_type,int16_t x_dist,int16_t y_dist,int16_t z_dist,uint16_t rect_col_num,float formation_heading)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
- _mav_put_uint32_t(buf, 0, timestamp);
- _mav_put_uint32_t(buf, 4, state);
- _mav_put_int32_t(buf, 8, lat);
- _mav_put_int32_t(buf, 12, lon);
- _mav_put_float(buf, 16, msl);
- _mav_put_float(buf, 20, ve);
- _mav_put_float(buf, 24, vn);
- _mav_put_float(buf, 28, vu);
- _mav_put_float(buf, 32, yaw);
- _mav_put_int16_t(buf, 36, x_dist);
- _mav_put_int16_t(buf, 38, y_dist);
- _mav_put_int16_t(buf, 40, z_dist);
- _mav_put_uint16_t(buf, 42, rect_col_num);
- _mav_put_uint8_t(buf, 44, formation_type);
- _mav_put_float(buf, 45, formation_heading);
- memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #else
- mavlink_vk_formation_leader_t packet;
- packet.timestamp = timestamp;
- packet.state = state;
- packet.lat = lat;
- packet.lon = lon;
- packet.msl = msl;
- packet.ve = ve;
- packet.vn = vn;
- packet.vu = vu;
- packet.yaw = yaw;
- packet.x_dist = x_dist;
- packet.y_dist = y_dist;
- packet.z_dist = z_dist;
- packet.rect_col_num = rect_col_num;
- packet.formation_type = formation_type;
- packet.formation_heading = formation_heading;
- memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- #endif
- msg->msgid = MAVLINK_MSG_ID_VK_FORMATION_LEADER;
- return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- }
- /**
- * @brief Encode a vk_formation_leader 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 vk_formation_leader C-struct to read the message contents from
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vk_formation_leader_t* vk_formation_leader)
- {
- return mavlink_msg_vk_formation_leader_pack(system_id, component_id, msg, vk_formation_leader->timestamp, vk_formation_leader->state, vk_formation_leader->lat, vk_formation_leader->lon, vk_formation_leader->msl, vk_formation_leader->ve, vk_formation_leader->vn, vk_formation_leader->vu, vk_formation_leader->yaw, vk_formation_leader->formation_type, vk_formation_leader->x_dist, vk_formation_leader->y_dist, vk_formation_leader->z_dist, vk_formation_leader->rect_col_num, vk_formation_leader->formation_heading);
- }
- /**
- * @brief Encode a vk_formation_leader 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 vk_formation_leader C-struct to read the message contents from
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vk_formation_leader_t* vk_formation_leader)
- {
- return mavlink_msg_vk_formation_leader_pack_chan(system_id, component_id, chan, msg, vk_formation_leader->timestamp, vk_formation_leader->state, vk_formation_leader->lat, vk_formation_leader->lon, vk_formation_leader->msl, vk_formation_leader->ve, vk_formation_leader->vn, vk_formation_leader->vu, vk_formation_leader->yaw, vk_formation_leader->formation_type, vk_formation_leader->x_dist, vk_formation_leader->y_dist, vk_formation_leader->z_dist, vk_formation_leader->rect_col_num, vk_formation_leader->formation_heading);
- }
- /**
- * @brief Encode a vk_formation_leader struct with provided status structure
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param status MAVLink status structure
- * @param msg The MAVLink message to compress the data into
- * @param vk_formation_leader C-struct to read the message contents from
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vk_formation_leader_t* vk_formation_leader)
- {
- return mavlink_msg_vk_formation_leader_pack_status(system_id, component_id, _status, msg, vk_formation_leader->timestamp, vk_formation_leader->state, vk_formation_leader->lat, vk_formation_leader->lon, vk_formation_leader->msl, vk_formation_leader->ve, vk_formation_leader->vn, vk_formation_leader->vu, vk_formation_leader->yaw, vk_formation_leader->formation_type, vk_formation_leader->x_dist, vk_formation_leader->y_dist, vk_formation_leader->z_dist, vk_formation_leader->rect_col_num, vk_formation_leader->formation_heading);
- }
- /**
- * @brief Send a vk_formation_leader message
- * @param chan MAVLink channel to send the message
- *
- * @param timestamp [ms] Timestamp in ms from system boot.
- * @param state formation leader drone state bitmap
- * @param lat [degE7] formation leader latitude in 1e-7deg
- * @param lon [degE7] formation leader longitude in 1e-7deg
- * @param msl [m] formation leader msl altitude in meter
- * @param ve [m/s] formation leader east speed
- * @param vn [m/s] formation leader north speed
- * @param vu [m/s] formation leader up speed
- * @param yaw [deg] formation leader yaw
- * @param formation_type formation type
- * @param x_dist [cm] distance between drones in x axis
- * @param y_dist [cm] distance between drones in y axis
- * @param z_dist [cm] distance between drones in z axis
- * @param rect_col_num columns number of rectangle formation
- * @param formation_heading [deg] if nan, use yaw as formation
- heading
- */
- #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
- static inline void mavlink_msg_vk_formation_leader_send(mavlink_channel_t chan, uint32_t timestamp, uint32_t state, int32_t lat, int32_t lon, float msl, float ve, float vn, float vu, float yaw, uint8_t formation_type, int16_t x_dist, int16_t y_dist, int16_t z_dist, uint16_t rect_col_num, float formation_heading)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
- _mav_put_uint32_t(buf, 0, timestamp);
- _mav_put_uint32_t(buf, 4, state);
- _mav_put_int32_t(buf, 8, lat);
- _mav_put_int32_t(buf, 12, lon);
- _mav_put_float(buf, 16, msl);
- _mav_put_float(buf, 20, ve);
- _mav_put_float(buf, 24, vn);
- _mav_put_float(buf, 28, vu);
- _mav_put_float(buf, 32, yaw);
- _mav_put_int16_t(buf, 36, x_dist);
- _mav_put_int16_t(buf, 38, y_dist);
- _mav_put_int16_t(buf, 40, z_dist);
- _mav_put_uint16_t(buf, 42, rect_col_num);
- _mav_put_uint8_t(buf, 44, formation_type);
- _mav_put_float(buf, 45, formation_heading);
- _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VK_FORMATION_LEADER, buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- #else
- mavlink_vk_formation_leader_t packet;
- packet.timestamp = timestamp;
- packet.state = state;
- packet.lat = lat;
- packet.lon = lon;
- packet.msl = msl;
- packet.ve = ve;
- packet.vn = vn;
- packet.vu = vu;
- packet.yaw = yaw;
- packet.x_dist = x_dist;
- packet.y_dist = y_dist;
- packet.z_dist = z_dist;
- packet.rect_col_num = rect_col_num;
- packet.formation_type = formation_type;
- packet.formation_heading = formation_heading;
- _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VK_FORMATION_LEADER, (const char *)&packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- #endif
- }
- /**
- * @brief Send a vk_formation_leader message
- * @param chan MAVLink channel to send the message
- * @param struct The MAVLink struct to serialize
- */
- static inline void mavlink_msg_vk_formation_leader_send_struct(mavlink_channel_t chan, const mavlink_vk_formation_leader_t* vk_formation_leader)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- mavlink_msg_vk_formation_leader_send(chan, vk_formation_leader->timestamp, vk_formation_leader->state, vk_formation_leader->lat, vk_formation_leader->lon, vk_formation_leader->msl, vk_formation_leader->ve, vk_formation_leader->vn, vk_formation_leader->vu, vk_formation_leader->yaw, vk_formation_leader->formation_type, vk_formation_leader->x_dist, vk_formation_leader->y_dist, vk_formation_leader->z_dist, vk_formation_leader->rect_col_num, vk_formation_leader->formation_heading);
- #else
- _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VK_FORMATION_LEADER, (const char *)vk_formation_leader, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- #endif
- }
- #if MAVLINK_MSG_ID_VK_FORMATION_LEADER_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_vk_formation_leader_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t timestamp, uint32_t state, int32_t lat, int32_t lon, float msl, float ve, float vn, float vu, float yaw, uint8_t formation_type, int16_t x_dist, int16_t y_dist, int16_t z_dist, uint16_t rect_col_num, float formation_heading)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- char *buf = (char *)msgbuf;
- _mav_put_uint32_t(buf, 0, timestamp);
- _mav_put_uint32_t(buf, 4, state);
- _mav_put_int32_t(buf, 8, lat);
- _mav_put_int32_t(buf, 12, lon);
- _mav_put_float(buf, 16, msl);
- _mav_put_float(buf, 20, ve);
- _mav_put_float(buf, 24, vn);
- _mav_put_float(buf, 28, vu);
- _mav_put_float(buf, 32, yaw);
- _mav_put_int16_t(buf, 36, x_dist);
- _mav_put_int16_t(buf, 38, y_dist);
- _mav_put_int16_t(buf, 40, z_dist);
- _mav_put_uint16_t(buf, 42, rect_col_num);
- _mav_put_uint8_t(buf, 44, formation_type);
- _mav_put_float(buf, 45, formation_heading);
- _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VK_FORMATION_LEADER, buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- #else
- mavlink_vk_formation_leader_t *packet = (mavlink_vk_formation_leader_t *)msgbuf;
- packet->timestamp = timestamp;
- packet->state = state;
- packet->lat = lat;
- packet->lon = lon;
- packet->msl = msl;
- packet->ve = ve;
- packet->vn = vn;
- packet->vu = vu;
- packet->yaw = yaw;
- packet->x_dist = x_dist;
- packet->y_dist = y_dist;
- packet->z_dist = z_dist;
- packet->rect_col_num = rect_col_num;
- packet->formation_type = formation_type;
- packet->formation_heading = formation_heading;
- _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VK_FORMATION_LEADER, (const char *)packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN, MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC);
- #endif
- }
- #endif
- #endif
- // MESSAGE VK_FORMATION_LEADER UNPACKING
- /**
- * @brief Get field timestamp from vk_formation_leader message
- *
- * @return [ms] Timestamp in ms from system boot.
- */
- static inline uint32_t mavlink_msg_vk_formation_leader_get_timestamp(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_uint32_t(msg, 0);
- }
- /**
- * @brief Get field state from vk_formation_leader message
- *
- * @return formation leader drone state bitmap
- */
- static inline uint32_t mavlink_msg_vk_formation_leader_get_state(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_uint32_t(msg, 4);
- }
- /**
- * @brief Get field lat from vk_formation_leader message
- *
- * @return [degE7] formation leader latitude in 1e-7deg
- */
- static inline int32_t mavlink_msg_vk_formation_leader_get_lat(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_int32_t(msg, 8);
- }
- /**
- * @brief Get field lon from vk_formation_leader message
- *
- * @return [degE7] formation leader longitude in 1e-7deg
- */
- static inline int32_t mavlink_msg_vk_formation_leader_get_lon(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_int32_t(msg, 12);
- }
- /**
- * @brief Get field msl from vk_formation_leader message
- *
- * @return [m] formation leader msl altitude in meter
- */
- static inline float mavlink_msg_vk_formation_leader_get_msl(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_float(msg, 16);
- }
- /**
- * @brief Get field ve from vk_formation_leader message
- *
- * @return [m/s] formation leader east speed
- */
- static inline float mavlink_msg_vk_formation_leader_get_ve(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_float(msg, 20);
- }
- /**
- * @brief Get field vn from vk_formation_leader message
- *
- * @return [m/s] formation leader north speed
- */
- static inline float mavlink_msg_vk_formation_leader_get_vn(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_float(msg, 24);
- }
- /**
- * @brief Get field vu from vk_formation_leader message
- *
- * @return [m/s] formation leader up speed
- */
- static inline float mavlink_msg_vk_formation_leader_get_vu(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_float(msg, 28);
- }
- /**
- * @brief Get field yaw from vk_formation_leader message
- *
- * @return [deg] formation leader yaw
- */
- static inline float mavlink_msg_vk_formation_leader_get_yaw(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_float(msg, 32);
- }
- /**
- * @brief Get field formation_type from vk_formation_leader message
- *
- * @return formation type
- */
- static inline uint8_t mavlink_msg_vk_formation_leader_get_formation_type(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_uint8_t(msg, 44);
- }
- /**
- * @brief Get field x_dist from vk_formation_leader message
- *
- * @return [cm] distance between drones in x axis
- */
- static inline int16_t mavlink_msg_vk_formation_leader_get_x_dist(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_int16_t(msg, 36);
- }
- /**
- * @brief Get field y_dist from vk_formation_leader message
- *
- * @return [cm] distance between drones in y axis
- */
- static inline int16_t mavlink_msg_vk_formation_leader_get_y_dist(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_int16_t(msg, 38);
- }
- /**
- * @brief Get field z_dist from vk_formation_leader message
- *
- * @return [cm] distance between drones in z axis
- */
- static inline int16_t mavlink_msg_vk_formation_leader_get_z_dist(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_int16_t(msg, 40);
- }
- /**
- * @brief Get field rect_col_num from vk_formation_leader message
- *
- * @return columns number of rectangle formation
- */
- static inline uint16_t mavlink_msg_vk_formation_leader_get_rect_col_num(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_uint16_t(msg, 42);
- }
- /**
- * @brief Get field formation_heading from vk_formation_leader message
- *
- * @return [deg] if nan, use yaw as formation
- heading
- */
- static inline float mavlink_msg_vk_formation_leader_get_formation_heading(const mavlink_message_t* msg)
- {
- return _MAV_RETURN_float(msg, 45);
- }
- /**
- * @brief Decode a vk_formation_leader message into a struct
- *
- * @param msg The message to decode
- * @param vk_formation_leader C-struct to decode the message contents into
- */
- static inline void mavlink_msg_vk_formation_leader_decode(const mavlink_message_t* msg, mavlink_vk_formation_leader_t* vk_formation_leader)
- {
- #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
- vk_formation_leader->timestamp = mavlink_msg_vk_formation_leader_get_timestamp(msg);
- vk_formation_leader->state = mavlink_msg_vk_formation_leader_get_state(msg);
- vk_formation_leader->lat = mavlink_msg_vk_formation_leader_get_lat(msg);
- vk_formation_leader->lon = mavlink_msg_vk_formation_leader_get_lon(msg);
- vk_formation_leader->msl = mavlink_msg_vk_formation_leader_get_msl(msg);
- vk_formation_leader->ve = mavlink_msg_vk_formation_leader_get_ve(msg);
- vk_formation_leader->vn = mavlink_msg_vk_formation_leader_get_vn(msg);
- vk_formation_leader->vu = mavlink_msg_vk_formation_leader_get_vu(msg);
- vk_formation_leader->yaw = mavlink_msg_vk_formation_leader_get_yaw(msg);
- vk_formation_leader->x_dist = mavlink_msg_vk_formation_leader_get_x_dist(msg);
- vk_formation_leader->y_dist = mavlink_msg_vk_formation_leader_get_y_dist(msg);
- vk_formation_leader->z_dist = mavlink_msg_vk_formation_leader_get_z_dist(msg);
- vk_formation_leader->rect_col_num = mavlink_msg_vk_formation_leader_get_rect_col_num(msg);
- vk_formation_leader->formation_type = mavlink_msg_vk_formation_leader_get_formation_type(msg);
- vk_formation_leader->formation_heading = mavlink_msg_vk_formation_leader_get_formation_heading(msg);
- #else
- uint8_t len = msg->len < MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN? msg->len : MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN;
- memset(vk_formation_leader, 0, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
- memcpy(vk_formation_leader, _MAV_PAYLOAD(msg), len);
- #endif
- }
|