mavlink_msg_vk_formation_leader.h 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658
  1. #pragma once
  2. // MESSAGE VK_FORMATION_LEADER PACKING
  3. #define MAVLINK_MSG_ID_VK_FORMATION_LEADER 53004
  4. MAVPACKED(
  5. typedef struct __mavlink_vk_formation_leader_t {
  6. uint32_t timestamp; /*< [ms] Timestamp in ms from system boot.*/
  7. uint32_t state; /*< formation leader drone state bitmap*/
  8. int32_t lat; /*< [degE7] formation leader latitude in 1e-7deg */
  9. int32_t lon; /*< [degE7] formation leader longitude in 1e-7deg*/
  10. float msl; /*< [m] formation leader msl altitude in meter*/
  11. float ve; /*< [m/s] formation leader east speed*/
  12. float vn; /*< [m/s] formation leader north speed*/
  13. float vu; /*< [m/s] formation leader up speed*/
  14. float yaw; /*< [deg] formation leader yaw*/
  15. int16_t x_dist; /*< [cm] distance between drones in x axis*/
  16. int16_t y_dist; /*< [cm] distance between drones in y axis*/
  17. int16_t z_dist; /*< [cm] distance between drones in z axis*/
  18. uint16_t rect_col_num; /*< columns number of rectangle formation*/
  19. uint8_t formation_type; /*< formation type*/
  20. float formation_heading; /*< [deg] if nan, use yaw as formation
  21. heading*/
  22. }) mavlink_vk_formation_leader_t;
  23. #define MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN 49
  24. #define MAVLINK_MSG_ID_VK_FORMATION_LEADER_MIN_LEN 45
  25. #define MAVLINK_MSG_ID_53004_LEN 49
  26. #define MAVLINK_MSG_ID_53004_MIN_LEN 45
  27. #define MAVLINK_MSG_ID_VK_FORMATION_LEADER_CRC 219
  28. #define MAVLINK_MSG_ID_53004_CRC 219
  29. #if MAVLINK_COMMAND_24BIT
  30. #define MAVLINK_MESSAGE_INFO_VK_FORMATION_LEADER { \
  31. 53004, \
  32. "VK_FORMATION_LEADER", \
  33. 15, \
  34. { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_vk_formation_leader_t, timestamp) }, \
  35. { "state", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_vk_formation_leader_t, state) }, \
  36. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_vk_formation_leader_t, lat) }, \
  37. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_vk_formation_leader_t, lon) }, \
  38. { "msl", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vk_formation_leader_t, msl) }, \
  39. { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vk_formation_leader_t, ve) }, \
  40. { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vk_formation_leader_t, vn) }, \
  41. { "vu", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vk_formation_leader_t, vu) }, \
  42. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_vk_formation_leader_t, yaw) }, \
  43. { "formation_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_vk_formation_leader_t, formation_type) }, \
  44. { "x_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_vk_formation_leader_t, x_dist) }, \
  45. { "y_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_vk_formation_leader_t, y_dist) }, \
  46. { "z_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_vk_formation_leader_t, z_dist) }, \
  47. { "rect_col_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_vk_formation_leader_t, rect_col_num) }, \
  48. { "formation_heading", NULL, MAVLINK_TYPE_FLOAT, 0, 45, offsetof(mavlink_vk_formation_leader_t, formation_heading) }, \
  49. } \
  50. }
  51. #else
  52. #define MAVLINK_MESSAGE_INFO_VK_FORMATION_LEADER { \
  53. "VK_FORMATION_LEADER", \
  54. 15, \
  55. { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_vk_formation_leader_t, timestamp) }, \
  56. { "state", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_vk_formation_leader_t, state) }, \
  57. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_vk_formation_leader_t, lat) }, \
  58. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_vk_formation_leader_t, lon) }, \
  59. { "msl", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vk_formation_leader_t, msl) }, \
  60. { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vk_formation_leader_t, ve) }, \
  61. { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vk_formation_leader_t, vn) }, \
  62. { "vu", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vk_formation_leader_t, vu) }, \
  63. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_vk_formation_leader_t, yaw) }, \
  64. { "formation_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_vk_formation_leader_t, formation_type) }, \
  65. { "x_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_vk_formation_leader_t, x_dist) }, \
  66. { "y_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_vk_formation_leader_t, y_dist) }, \
  67. { "z_dist", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_vk_formation_leader_t, z_dist) }, \
  68. { "rect_col_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_vk_formation_leader_t, rect_col_num) }, \
  69. { "formation_heading", NULL, MAVLINK_TYPE_FLOAT, 0, 45, offsetof(mavlink_vk_formation_leader_t, formation_heading) }, \
  70. } \
  71. }
  72. #endif
  73. /**
  74. * @brief Pack a vk_formation_leader message
  75. * @param system_id ID of this system
  76. * @param component_id ID of this component (e.g. 200 for IMU)
  77. * @param msg The MAVLink message to compress the data into
  78. *
  79. * @param timestamp [ms] Timestamp in ms from system boot.
  80. * @param state formation leader drone state bitmap
  81. * @param lat [degE7] formation leader latitude in 1e-7deg
  82. * @param lon [degE7] formation leader longitude in 1e-7deg
  83. * @param msl [m] formation leader msl altitude in meter
  84. * @param ve [m/s] formation leader east speed
  85. * @param vn [m/s] formation leader north speed
  86. * @param vu [m/s] formation leader up speed
  87. * @param yaw [deg] formation leader yaw
  88. * @param formation_type formation type
  89. * @param x_dist [cm] distance between drones in x axis
  90. * @param y_dist [cm] distance between drones in y axis
  91. * @param z_dist [cm] distance between drones in z axis
  92. * @param rect_col_num columns number of rectangle formation
  93. * @param formation_heading [deg] if nan, use yaw as formation
  94. heading
  95. * @return length of the message in bytes (excluding serial stream start sign)
  96. */
  97. static inline uint16_t mavlink_msg_vk_formation_leader_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  98. 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)
  99. {
  100. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  101. char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
  102. _mav_put_uint32_t(buf, 0, timestamp);
  103. _mav_put_uint32_t(buf, 4, state);
  104. _mav_put_int32_t(buf, 8, lat);
  105. _mav_put_int32_t(buf, 12, lon);
  106. _mav_put_float(buf, 16, msl);
  107. _mav_put_float(buf, 20, ve);
  108. _mav_put_float(buf, 24, vn);
  109. _mav_put_float(buf, 28, vu);
  110. _mav_put_float(buf, 32, yaw);
  111. _mav_put_int16_t(buf, 36, x_dist);
  112. _mav_put_int16_t(buf, 38, y_dist);
  113. _mav_put_int16_t(buf, 40, z_dist);
  114. _mav_put_uint16_t(buf, 42, rect_col_num);
  115. _mav_put_uint8_t(buf, 44, formation_type);
  116. _mav_put_float(buf, 45, formation_heading);
  117. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  118. #else
  119. mavlink_vk_formation_leader_t packet;
  120. packet.timestamp = timestamp;
  121. packet.state = state;
  122. packet.lat = lat;
  123. packet.lon = lon;
  124. packet.msl = msl;
  125. packet.ve = ve;
  126. packet.vn = vn;
  127. packet.vu = vu;
  128. packet.yaw = yaw;
  129. packet.x_dist = x_dist;
  130. packet.y_dist = y_dist;
  131. packet.z_dist = z_dist;
  132. packet.rect_col_num = rect_col_num;
  133. packet.formation_type = formation_type;
  134. packet.formation_heading = formation_heading;
  135. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  136. #endif
  137. msg->msgid = MAVLINK_MSG_ID_VK_FORMATION_LEADER;
  138. 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);
  139. }
  140. /**
  141. * @brief Pack a vk_formation_leader message
  142. * @param system_id ID of this system
  143. * @param component_id ID of this component (e.g. 200 for IMU)
  144. * @param status MAVLink status structure
  145. * @param msg The MAVLink message to compress the data into
  146. *
  147. * @param timestamp [ms] Timestamp in ms from system boot.
  148. * @param state formation leader drone state bitmap
  149. * @param lat [degE7] formation leader latitude in 1e-7deg
  150. * @param lon [degE7] formation leader longitude in 1e-7deg
  151. * @param msl [m] formation leader msl altitude in meter
  152. * @param ve [m/s] formation leader east speed
  153. * @param vn [m/s] formation leader north speed
  154. * @param vu [m/s] formation leader up speed
  155. * @param yaw [deg] formation leader yaw
  156. * @param formation_type formation type
  157. * @param x_dist [cm] distance between drones in x axis
  158. * @param y_dist [cm] distance between drones in y axis
  159. * @param z_dist [cm] distance between drones in z axis
  160. * @param rect_col_num columns number of rectangle formation
  161. * @param formation_heading [deg] if nan, use yaw as formation
  162. heading
  163. * @return length of the message in bytes (excluding serial stream start sign)
  164. */
  165. 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,
  166. 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)
  167. {
  168. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  169. char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
  170. _mav_put_uint32_t(buf, 0, timestamp);
  171. _mav_put_uint32_t(buf, 4, state);
  172. _mav_put_int32_t(buf, 8, lat);
  173. _mav_put_int32_t(buf, 12, lon);
  174. _mav_put_float(buf, 16, msl);
  175. _mav_put_float(buf, 20, ve);
  176. _mav_put_float(buf, 24, vn);
  177. _mav_put_float(buf, 28, vu);
  178. _mav_put_float(buf, 32, yaw);
  179. _mav_put_int16_t(buf, 36, x_dist);
  180. _mav_put_int16_t(buf, 38, y_dist);
  181. _mav_put_int16_t(buf, 40, z_dist);
  182. _mav_put_uint16_t(buf, 42, rect_col_num);
  183. _mav_put_uint8_t(buf, 44, formation_type);
  184. _mav_put_float(buf, 45, formation_heading);
  185. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  186. #else
  187. mavlink_vk_formation_leader_t packet;
  188. packet.timestamp = timestamp;
  189. packet.state = state;
  190. packet.lat = lat;
  191. packet.lon = lon;
  192. packet.msl = msl;
  193. packet.ve = ve;
  194. packet.vn = vn;
  195. packet.vu = vu;
  196. packet.yaw = yaw;
  197. packet.x_dist = x_dist;
  198. packet.y_dist = y_dist;
  199. packet.z_dist = z_dist;
  200. packet.rect_col_num = rect_col_num;
  201. packet.formation_type = formation_type;
  202. packet.formation_heading = formation_heading;
  203. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  204. #endif
  205. msg->msgid = MAVLINK_MSG_ID_VK_FORMATION_LEADER;
  206. #if MAVLINK_CRC_EXTRA
  207. 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);
  208. #else
  209. 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);
  210. #endif
  211. }
  212. /**
  213. * @brief Pack a vk_formation_leader message on a channel
  214. * @param system_id ID of this system
  215. * @param component_id ID of this component (e.g. 200 for IMU)
  216. * @param chan The MAVLink channel this message will be sent over
  217. * @param msg The MAVLink message to compress the data into
  218. * @param timestamp [ms] Timestamp in ms from system boot.
  219. * @param state formation leader drone state bitmap
  220. * @param lat [degE7] formation leader latitude in 1e-7deg
  221. * @param lon [degE7] formation leader longitude in 1e-7deg
  222. * @param msl [m] formation leader msl altitude in meter
  223. * @param ve [m/s] formation leader east speed
  224. * @param vn [m/s] formation leader north speed
  225. * @param vu [m/s] formation leader up speed
  226. * @param yaw [deg] formation leader yaw
  227. * @param formation_type formation type
  228. * @param x_dist [cm] distance between drones in x axis
  229. * @param y_dist [cm] distance between drones in y axis
  230. * @param z_dist [cm] distance between drones in z axis
  231. * @param rect_col_num columns number of rectangle formation
  232. * @param formation_heading [deg] if nan, use yaw as formation
  233. heading
  234. * @return length of the message in bytes (excluding serial stream start sign)
  235. */
  236. static inline uint16_t mavlink_msg_vk_formation_leader_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  237. mavlink_message_t* msg,
  238. 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)
  239. {
  240. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  241. char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
  242. _mav_put_uint32_t(buf, 0, timestamp);
  243. _mav_put_uint32_t(buf, 4, state);
  244. _mav_put_int32_t(buf, 8, lat);
  245. _mav_put_int32_t(buf, 12, lon);
  246. _mav_put_float(buf, 16, msl);
  247. _mav_put_float(buf, 20, ve);
  248. _mav_put_float(buf, 24, vn);
  249. _mav_put_float(buf, 28, vu);
  250. _mav_put_float(buf, 32, yaw);
  251. _mav_put_int16_t(buf, 36, x_dist);
  252. _mav_put_int16_t(buf, 38, y_dist);
  253. _mav_put_int16_t(buf, 40, z_dist);
  254. _mav_put_uint16_t(buf, 42, rect_col_num);
  255. _mav_put_uint8_t(buf, 44, formation_type);
  256. _mav_put_float(buf, 45, formation_heading);
  257. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  258. #else
  259. mavlink_vk_formation_leader_t packet;
  260. packet.timestamp = timestamp;
  261. packet.state = state;
  262. packet.lat = lat;
  263. packet.lon = lon;
  264. packet.msl = msl;
  265. packet.ve = ve;
  266. packet.vn = vn;
  267. packet.vu = vu;
  268. packet.yaw = yaw;
  269. packet.x_dist = x_dist;
  270. packet.y_dist = y_dist;
  271. packet.z_dist = z_dist;
  272. packet.rect_col_num = rect_col_num;
  273. packet.formation_type = formation_type;
  274. packet.formation_heading = formation_heading;
  275. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  276. #endif
  277. msg->msgid = MAVLINK_MSG_ID_VK_FORMATION_LEADER;
  278. 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);
  279. }
  280. /**
  281. * @brief Encode a vk_formation_leader struct
  282. *
  283. * @param system_id ID of this system
  284. * @param component_id ID of this component (e.g. 200 for IMU)
  285. * @param msg The MAVLink message to compress the data into
  286. * @param vk_formation_leader C-struct to read the message contents from
  287. */
  288. 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)
  289. {
  290. 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);
  291. }
  292. /**
  293. * @brief Encode a vk_formation_leader struct on a channel
  294. *
  295. * @param system_id ID of this system
  296. * @param component_id ID of this component (e.g. 200 for IMU)
  297. * @param chan The MAVLink channel this message will be sent over
  298. * @param msg The MAVLink message to compress the data into
  299. * @param vk_formation_leader C-struct to read the message contents from
  300. */
  301. 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)
  302. {
  303. 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);
  304. }
  305. /**
  306. * @brief Encode a vk_formation_leader struct with provided status structure
  307. *
  308. * @param system_id ID of this system
  309. * @param component_id ID of this component (e.g. 200 for IMU)
  310. * @param status MAVLink status structure
  311. * @param msg The MAVLink message to compress the data into
  312. * @param vk_formation_leader C-struct to read the message contents from
  313. */
  314. 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)
  315. {
  316. 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);
  317. }
  318. /**
  319. * @brief Send a vk_formation_leader message
  320. * @param chan MAVLink channel to send the message
  321. *
  322. * @param timestamp [ms] Timestamp in ms from system boot.
  323. * @param state formation leader drone state bitmap
  324. * @param lat [degE7] formation leader latitude in 1e-7deg
  325. * @param lon [degE7] formation leader longitude in 1e-7deg
  326. * @param msl [m] formation leader msl altitude in meter
  327. * @param ve [m/s] formation leader east speed
  328. * @param vn [m/s] formation leader north speed
  329. * @param vu [m/s] formation leader up speed
  330. * @param yaw [deg] formation leader yaw
  331. * @param formation_type formation type
  332. * @param x_dist [cm] distance between drones in x axis
  333. * @param y_dist [cm] distance between drones in y axis
  334. * @param z_dist [cm] distance between drones in z axis
  335. * @param rect_col_num columns number of rectangle formation
  336. * @param formation_heading [deg] if nan, use yaw as formation
  337. heading
  338. */
  339. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  340. 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)
  341. {
  342. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  343. char buf[MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN];
  344. _mav_put_uint32_t(buf, 0, timestamp);
  345. _mav_put_uint32_t(buf, 4, state);
  346. _mav_put_int32_t(buf, 8, lat);
  347. _mav_put_int32_t(buf, 12, lon);
  348. _mav_put_float(buf, 16, msl);
  349. _mav_put_float(buf, 20, ve);
  350. _mav_put_float(buf, 24, vn);
  351. _mav_put_float(buf, 28, vu);
  352. _mav_put_float(buf, 32, yaw);
  353. _mav_put_int16_t(buf, 36, x_dist);
  354. _mav_put_int16_t(buf, 38, y_dist);
  355. _mav_put_int16_t(buf, 40, z_dist);
  356. _mav_put_uint16_t(buf, 42, rect_col_num);
  357. _mav_put_uint8_t(buf, 44, formation_type);
  358. _mav_put_float(buf, 45, formation_heading);
  359. _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);
  360. #else
  361. mavlink_vk_formation_leader_t packet;
  362. packet.timestamp = timestamp;
  363. packet.state = state;
  364. packet.lat = lat;
  365. packet.lon = lon;
  366. packet.msl = msl;
  367. packet.ve = ve;
  368. packet.vn = vn;
  369. packet.vu = vu;
  370. packet.yaw = yaw;
  371. packet.x_dist = x_dist;
  372. packet.y_dist = y_dist;
  373. packet.z_dist = z_dist;
  374. packet.rect_col_num = rect_col_num;
  375. packet.formation_type = formation_type;
  376. packet.formation_heading = formation_heading;
  377. _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);
  378. #endif
  379. }
  380. /**
  381. * @brief Send a vk_formation_leader message
  382. * @param chan MAVLink channel to send the message
  383. * @param struct The MAVLink struct to serialize
  384. */
  385. static inline void mavlink_msg_vk_formation_leader_send_struct(mavlink_channel_t chan, const mavlink_vk_formation_leader_t* vk_formation_leader)
  386. {
  387. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  388. 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);
  389. #else
  390. _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);
  391. #endif
  392. }
  393. #if MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  394. /*
  395. This variant of _send() can be used to save stack space by re-using
  396. memory from the receive buffer. The caller provides a
  397. mavlink_message_t which is the size of a full mavlink message. This
  398. is usually the receive buffer for the channel, and allows a reply to an
  399. incoming message with minimum stack space usage.
  400. */
  401. 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)
  402. {
  403. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  404. char *buf = (char *)msgbuf;
  405. _mav_put_uint32_t(buf, 0, timestamp);
  406. _mav_put_uint32_t(buf, 4, state);
  407. _mav_put_int32_t(buf, 8, lat);
  408. _mav_put_int32_t(buf, 12, lon);
  409. _mav_put_float(buf, 16, msl);
  410. _mav_put_float(buf, 20, ve);
  411. _mav_put_float(buf, 24, vn);
  412. _mav_put_float(buf, 28, vu);
  413. _mav_put_float(buf, 32, yaw);
  414. _mav_put_int16_t(buf, 36, x_dist);
  415. _mav_put_int16_t(buf, 38, y_dist);
  416. _mav_put_int16_t(buf, 40, z_dist);
  417. _mav_put_uint16_t(buf, 42, rect_col_num);
  418. _mav_put_uint8_t(buf, 44, formation_type);
  419. _mav_put_float(buf, 45, formation_heading);
  420. _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);
  421. #else
  422. mavlink_vk_formation_leader_t *packet = (mavlink_vk_formation_leader_t *)msgbuf;
  423. packet->timestamp = timestamp;
  424. packet->state = state;
  425. packet->lat = lat;
  426. packet->lon = lon;
  427. packet->msl = msl;
  428. packet->ve = ve;
  429. packet->vn = vn;
  430. packet->vu = vu;
  431. packet->yaw = yaw;
  432. packet->x_dist = x_dist;
  433. packet->y_dist = y_dist;
  434. packet->z_dist = z_dist;
  435. packet->rect_col_num = rect_col_num;
  436. packet->formation_type = formation_type;
  437. packet->formation_heading = formation_heading;
  438. _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);
  439. #endif
  440. }
  441. #endif
  442. #endif
  443. // MESSAGE VK_FORMATION_LEADER UNPACKING
  444. /**
  445. * @brief Get field timestamp from vk_formation_leader message
  446. *
  447. * @return [ms] Timestamp in ms from system boot.
  448. */
  449. static inline uint32_t mavlink_msg_vk_formation_leader_get_timestamp(const mavlink_message_t* msg)
  450. {
  451. return _MAV_RETURN_uint32_t(msg, 0);
  452. }
  453. /**
  454. * @brief Get field state from vk_formation_leader message
  455. *
  456. * @return formation leader drone state bitmap
  457. */
  458. static inline uint32_t mavlink_msg_vk_formation_leader_get_state(const mavlink_message_t* msg)
  459. {
  460. return _MAV_RETURN_uint32_t(msg, 4);
  461. }
  462. /**
  463. * @brief Get field lat from vk_formation_leader message
  464. *
  465. * @return [degE7] formation leader latitude in 1e-7deg
  466. */
  467. static inline int32_t mavlink_msg_vk_formation_leader_get_lat(const mavlink_message_t* msg)
  468. {
  469. return _MAV_RETURN_int32_t(msg, 8);
  470. }
  471. /**
  472. * @brief Get field lon from vk_formation_leader message
  473. *
  474. * @return [degE7] formation leader longitude in 1e-7deg
  475. */
  476. static inline int32_t mavlink_msg_vk_formation_leader_get_lon(const mavlink_message_t* msg)
  477. {
  478. return _MAV_RETURN_int32_t(msg, 12);
  479. }
  480. /**
  481. * @brief Get field msl from vk_formation_leader message
  482. *
  483. * @return [m] formation leader msl altitude in meter
  484. */
  485. static inline float mavlink_msg_vk_formation_leader_get_msl(const mavlink_message_t* msg)
  486. {
  487. return _MAV_RETURN_float(msg, 16);
  488. }
  489. /**
  490. * @brief Get field ve from vk_formation_leader message
  491. *
  492. * @return [m/s] formation leader east speed
  493. */
  494. static inline float mavlink_msg_vk_formation_leader_get_ve(const mavlink_message_t* msg)
  495. {
  496. return _MAV_RETURN_float(msg, 20);
  497. }
  498. /**
  499. * @brief Get field vn from vk_formation_leader message
  500. *
  501. * @return [m/s] formation leader north speed
  502. */
  503. static inline float mavlink_msg_vk_formation_leader_get_vn(const mavlink_message_t* msg)
  504. {
  505. return _MAV_RETURN_float(msg, 24);
  506. }
  507. /**
  508. * @brief Get field vu from vk_formation_leader message
  509. *
  510. * @return [m/s] formation leader up speed
  511. */
  512. static inline float mavlink_msg_vk_formation_leader_get_vu(const mavlink_message_t* msg)
  513. {
  514. return _MAV_RETURN_float(msg, 28);
  515. }
  516. /**
  517. * @brief Get field yaw from vk_formation_leader message
  518. *
  519. * @return [deg] formation leader yaw
  520. */
  521. static inline float mavlink_msg_vk_formation_leader_get_yaw(const mavlink_message_t* msg)
  522. {
  523. return _MAV_RETURN_float(msg, 32);
  524. }
  525. /**
  526. * @brief Get field formation_type from vk_formation_leader message
  527. *
  528. * @return formation type
  529. */
  530. static inline uint8_t mavlink_msg_vk_formation_leader_get_formation_type(const mavlink_message_t* msg)
  531. {
  532. return _MAV_RETURN_uint8_t(msg, 44);
  533. }
  534. /**
  535. * @brief Get field x_dist from vk_formation_leader message
  536. *
  537. * @return [cm] distance between drones in x axis
  538. */
  539. static inline int16_t mavlink_msg_vk_formation_leader_get_x_dist(const mavlink_message_t* msg)
  540. {
  541. return _MAV_RETURN_int16_t(msg, 36);
  542. }
  543. /**
  544. * @brief Get field y_dist from vk_formation_leader message
  545. *
  546. * @return [cm] distance between drones in y axis
  547. */
  548. static inline int16_t mavlink_msg_vk_formation_leader_get_y_dist(const mavlink_message_t* msg)
  549. {
  550. return _MAV_RETURN_int16_t(msg, 38);
  551. }
  552. /**
  553. * @brief Get field z_dist from vk_formation_leader message
  554. *
  555. * @return [cm] distance between drones in z axis
  556. */
  557. static inline int16_t mavlink_msg_vk_formation_leader_get_z_dist(const mavlink_message_t* msg)
  558. {
  559. return _MAV_RETURN_int16_t(msg, 40);
  560. }
  561. /**
  562. * @brief Get field rect_col_num from vk_formation_leader message
  563. *
  564. * @return columns number of rectangle formation
  565. */
  566. static inline uint16_t mavlink_msg_vk_formation_leader_get_rect_col_num(const mavlink_message_t* msg)
  567. {
  568. return _MAV_RETURN_uint16_t(msg, 42);
  569. }
  570. /**
  571. * @brief Get field formation_heading from vk_formation_leader message
  572. *
  573. * @return [deg] if nan, use yaw as formation
  574. heading
  575. */
  576. static inline float mavlink_msg_vk_formation_leader_get_formation_heading(const mavlink_message_t* msg)
  577. {
  578. return _MAV_RETURN_float(msg, 45);
  579. }
  580. /**
  581. * @brief Decode a vk_formation_leader message into a struct
  582. *
  583. * @param msg The message to decode
  584. * @param vk_formation_leader C-struct to decode the message contents into
  585. */
  586. static inline void mavlink_msg_vk_formation_leader_decode(const mavlink_message_t* msg, mavlink_vk_formation_leader_t* vk_formation_leader)
  587. {
  588. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  589. vk_formation_leader->timestamp = mavlink_msg_vk_formation_leader_get_timestamp(msg);
  590. vk_formation_leader->state = mavlink_msg_vk_formation_leader_get_state(msg);
  591. vk_formation_leader->lat = mavlink_msg_vk_formation_leader_get_lat(msg);
  592. vk_formation_leader->lon = mavlink_msg_vk_formation_leader_get_lon(msg);
  593. vk_formation_leader->msl = mavlink_msg_vk_formation_leader_get_msl(msg);
  594. vk_formation_leader->ve = mavlink_msg_vk_formation_leader_get_ve(msg);
  595. vk_formation_leader->vn = mavlink_msg_vk_formation_leader_get_vn(msg);
  596. vk_formation_leader->vu = mavlink_msg_vk_formation_leader_get_vu(msg);
  597. vk_formation_leader->yaw = mavlink_msg_vk_formation_leader_get_yaw(msg);
  598. vk_formation_leader->x_dist = mavlink_msg_vk_formation_leader_get_x_dist(msg);
  599. vk_formation_leader->y_dist = mavlink_msg_vk_formation_leader_get_y_dist(msg);
  600. vk_formation_leader->z_dist = mavlink_msg_vk_formation_leader_get_z_dist(msg);
  601. vk_formation_leader->rect_col_num = mavlink_msg_vk_formation_leader_get_rect_col_num(msg);
  602. vk_formation_leader->formation_type = mavlink_msg_vk_formation_leader_get_formation_type(msg);
  603. vk_formation_leader->formation_heading = mavlink_msg_vk_formation_leader_get_formation_heading(msg);
  604. #else
  605. uint8_t len = msg->len < MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN? msg->len : MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN;
  606. memset(vk_formation_leader, 0, MAVLINK_MSG_ID_VK_FORMATION_LEADER_LEN);
  607. memcpy(vk_formation_leader, _MAV_PAYLOAD(msg), len);
  608. #endif
  609. }