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