mavlink_msg_utm_global_position.h 34 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726
  1. #pragma once
  2. // MESSAGE UTM_GLOBAL_POSITION PACKING
  3. #define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION 340
  4. typedef struct __mavlink_utm_global_position_t {
  5. uint64_t time; /*< [us] Time of applicability of position (microseconds since UNIX epoch).*/
  6. int32_t lat; /*< [degE7] Latitude (WGS84)*/
  7. int32_t lon; /*< [degE7] Longitude (WGS84)*/
  8. int32_t alt; /*< [mm] Altitude (WGS84)*/
  9. int32_t relative_alt; /*< [mm] Altitude above ground*/
  10. int32_t next_lat; /*< [degE7] Next waypoint, latitude (WGS84)*/
  11. int32_t next_lon; /*< [degE7] Next waypoint, longitude (WGS84)*/
  12. int32_t next_alt; /*< [mm] Next waypoint, altitude (WGS84)*/
  13. int16_t vx; /*< [cm/s] Ground X speed (latitude, positive north)*/
  14. int16_t vy; /*< [cm/s] Ground Y speed (longitude, positive east)*/
  15. int16_t vz; /*< [cm/s] Ground Z speed (altitude, positive down)*/
  16. uint16_t h_acc; /*< [mm] Horizontal position uncertainty (standard deviation)*/
  17. uint16_t v_acc; /*< [mm] Altitude uncertainty (standard deviation)*/
  18. uint16_t vel_acc; /*< [cm/s] Speed uncertainty (standard deviation)*/
  19. uint16_t update_rate; /*< [cs] Time until next update. Set to 0 if unknown or in data driven mode.*/
  20. uint8_t uas_id[18]; /*< Unique UAS ID.*/
  21. uint8_t flight_state; /*< Flight state*/
  22. uint8_t flags; /*< Bitwise OR combination of the data available flags.*/
  23. } mavlink_utm_global_position_t;
  24. #define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN 70
  25. #define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN 70
  26. #define MAVLINK_MSG_ID_340_LEN 70
  27. #define MAVLINK_MSG_ID_340_MIN_LEN 70
  28. #define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC 99
  29. #define MAVLINK_MSG_ID_340_CRC 99
  30. #define MAVLINK_MSG_UTM_GLOBAL_POSITION_FIELD_UAS_ID_LEN 18
  31. #if MAVLINK_COMMAND_24BIT
  32. #define MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION { \
  33. 340, \
  34. "UTM_GLOBAL_POSITION", \
  35. 18, \
  36. { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_utm_global_position_t, time) }, \
  37. { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 18, 50, offsetof(mavlink_utm_global_position_t, uas_id) }, \
  38. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_utm_global_position_t, lat) }, \
  39. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_utm_global_position_t, lon) }, \
  40. { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_utm_global_position_t, alt) }, \
  41. { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_utm_global_position_t, relative_alt) }, \
  42. { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_utm_global_position_t, vx) }, \
  43. { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_utm_global_position_t, vy) }, \
  44. { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_utm_global_position_t, vz) }, \
  45. { "h_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_utm_global_position_t, h_acc) }, \
  46. { "v_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_utm_global_position_t, v_acc) }, \
  47. { "vel_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_utm_global_position_t, vel_acc) }, \
  48. { "next_lat", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_utm_global_position_t, next_lat) }, \
  49. { "next_lon", NULL, MAVLINK_TYPE_INT32_T, 0, 28, offsetof(mavlink_utm_global_position_t, next_lon) }, \
  50. { "next_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_utm_global_position_t, next_alt) }, \
  51. { "update_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_utm_global_position_t, update_rate) }, \
  52. { "flight_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 68, offsetof(mavlink_utm_global_position_t, flight_state) }, \
  53. { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 69, offsetof(mavlink_utm_global_position_t, flags) }, \
  54. } \
  55. }
  56. #else
  57. #define MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION { \
  58. "UTM_GLOBAL_POSITION", \
  59. 18, \
  60. { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_utm_global_position_t, time) }, \
  61. { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 18, 50, offsetof(mavlink_utm_global_position_t, uas_id) }, \
  62. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_utm_global_position_t, lat) }, \
  63. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_utm_global_position_t, lon) }, \
  64. { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_utm_global_position_t, alt) }, \
  65. { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_utm_global_position_t, relative_alt) }, \
  66. { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_utm_global_position_t, vx) }, \
  67. { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_utm_global_position_t, vy) }, \
  68. { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_utm_global_position_t, vz) }, \
  69. { "h_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_utm_global_position_t, h_acc) }, \
  70. { "v_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_utm_global_position_t, v_acc) }, \
  71. { "vel_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_utm_global_position_t, vel_acc) }, \
  72. { "next_lat", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_utm_global_position_t, next_lat) }, \
  73. { "next_lon", NULL, MAVLINK_TYPE_INT32_T, 0, 28, offsetof(mavlink_utm_global_position_t, next_lon) }, \
  74. { "next_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_utm_global_position_t, next_alt) }, \
  75. { "update_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_utm_global_position_t, update_rate) }, \
  76. { "flight_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 68, offsetof(mavlink_utm_global_position_t, flight_state) }, \
  77. { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 69, offsetof(mavlink_utm_global_position_t, flags) }, \
  78. } \
  79. }
  80. #endif
  81. /**
  82. * @brief Pack a utm_global_position message
  83. * @param system_id ID of this system
  84. * @param component_id ID of this component (e.g. 200 for IMU)
  85. * @param msg The MAVLink message to compress the data into
  86. *
  87. * @param time [us] Time of applicability of position (microseconds since UNIX epoch).
  88. * @param uas_id Unique UAS ID.
  89. * @param lat [degE7] Latitude (WGS84)
  90. * @param lon [degE7] Longitude (WGS84)
  91. * @param alt [mm] Altitude (WGS84)
  92. * @param relative_alt [mm] Altitude above ground
  93. * @param vx [cm/s] Ground X speed (latitude, positive north)
  94. * @param vy [cm/s] Ground Y speed (longitude, positive east)
  95. * @param vz [cm/s] Ground Z speed (altitude, positive down)
  96. * @param h_acc [mm] Horizontal position uncertainty (standard deviation)
  97. * @param v_acc [mm] Altitude uncertainty (standard deviation)
  98. * @param vel_acc [cm/s] Speed uncertainty (standard deviation)
  99. * @param next_lat [degE7] Next waypoint, latitude (WGS84)
  100. * @param next_lon [degE7] Next waypoint, longitude (WGS84)
  101. * @param next_alt [mm] Next waypoint, altitude (WGS84)
  102. * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode.
  103. * @param flight_state Flight state
  104. * @param flags Bitwise OR combination of the data available flags.
  105. * @return length of the message in bytes (excluding serial stream start sign)
  106. */
  107. static inline uint16_t mavlink_msg_utm_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  108. uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags)
  109. {
  110. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  111. char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN];
  112. _mav_put_uint64_t(buf, 0, time);
  113. _mav_put_int32_t(buf, 8, lat);
  114. _mav_put_int32_t(buf, 12, lon);
  115. _mav_put_int32_t(buf, 16, alt);
  116. _mav_put_int32_t(buf, 20, relative_alt);
  117. _mav_put_int32_t(buf, 24, next_lat);
  118. _mav_put_int32_t(buf, 28, next_lon);
  119. _mav_put_int32_t(buf, 32, next_alt);
  120. _mav_put_int16_t(buf, 36, vx);
  121. _mav_put_int16_t(buf, 38, vy);
  122. _mav_put_int16_t(buf, 40, vz);
  123. _mav_put_uint16_t(buf, 42, h_acc);
  124. _mav_put_uint16_t(buf, 44, v_acc);
  125. _mav_put_uint16_t(buf, 46, vel_acc);
  126. _mav_put_uint16_t(buf, 48, update_rate);
  127. _mav_put_uint8_t(buf, 68, flight_state);
  128. _mav_put_uint8_t(buf, 69, flags);
  129. _mav_put_uint8_t_array(buf, 50, uas_id, 18);
  130. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  131. #else
  132. mavlink_utm_global_position_t packet;
  133. packet.time = time;
  134. packet.lat = lat;
  135. packet.lon = lon;
  136. packet.alt = alt;
  137. packet.relative_alt = relative_alt;
  138. packet.next_lat = next_lat;
  139. packet.next_lon = next_lon;
  140. packet.next_alt = next_alt;
  141. packet.vx = vx;
  142. packet.vy = vy;
  143. packet.vz = vz;
  144. packet.h_acc = h_acc;
  145. packet.v_acc = v_acc;
  146. packet.vel_acc = vel_acc;
  147. packet.update_rate = update_rate;
  148. packet.flight_state = flight_state;
  149. packet.flags = flags;
  150. mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18);
  151. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  152. #endif
  153. msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION;
  154. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  155. }
  156. /**
  157. * @brief Pack a utm_global_position message
  158. * @param system_id ID of this system
  159. * @param component_id ID of this component (e.g. 200 for IMU)
  160. * @param status MAVLink status structure
  161. * @param msg The MAVLink message to compress the data into
  162. *
  163. * @param time [us] Time of applicability of position (microseconds since UNIX epoch).
  164. * @param uas_id Unique UAS ID.
  165. * @param lat [degE7] Latitude (WGS84)
  166. * @param lon [degE7] Longitude (WGS84)
  167. * @param alt [mm] Altitude (WGS84)
  168. * @param relative_alt [mm] Altitude above ground
  169. * @param vx [cm/s] Ground X speed (latitude, positive north)
  170. * @param vy [cm/s] Ground Y speed (longitude, positive east)
  171. * @param vz [cm/s] Ground Z speed (altitude, positive down)
  172. * @param h_acc [mm] Horizontal position uncertainty (standard deviation)
  173. * @param v_acc [mm] Altitude uncertainty (standard deviation)
  174. * @param vel_acc [cm/s] Speed uncertainty (standard deviation)
  175. * @param next_lat [degE7] Next waypoint, latitude (WGS84)
  176. * @param next_lon [degE7] Next waypoint, longitude (WGS84)
  177. * @param next_alt [mm] Next waypoint, altitude (WGS84)
  178. * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode.
  179. * @param flight_state Flight state
  180. * @param flags Bitwise OR combination of the data available flags.
  181. * @return length of the message in bytes (excluding serial stream start sign)
  182. */
  183. static inline uint16_t mavlink_msg_utm_global_position_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
  184. uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags)
  185. {
  186. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  187. char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN];
  188. _mav_put_uint64_t(buf, 0, time);
  189. _mav_put_int32_t(buf, 8, lat);
  190. _mav_put_int32_t(buf, 12, lon);
  191. _mav_put_int32_t(buf, 16, alt);
  192. _mav_put_int32_t(buf, 20, relative_alt);
  193. _mav_put_int32_t(buf, 24, next_lat);
  194. _mav_put_int32_t(buf, 28, next_lon);
  195. _mav_put_int32_t(buf, 32, next_alt);
  196. _mav_put_int16_t(buf, 36, vx);
  197. _mav_put_int16_t(buf, 38, vy);
  198. _mav_put_int16_t(buf, 40, vz);
  199. _mav_put_uint16_t(buf, 42, h_acc);
  200. _mav_put_uint16_t(buf, 44, v_acc);
  201. _mav_put_uint16_t(buf, 46, vel_acc);
  202. _mav_put_uint16_t(buf, 48, update_rate);
  203. _mav_put_uint8_t(buf, 68, flight_state);
  204. _mav_put_uint8_t(buf, 69, flags);
  205. _mav_put_uint8_t_array(buf, 50, uas_id, 18);
  206. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  207. #else
  208. mavlink_utm_global_position_t packet;
  209. packet.time = time;
  210. packet.lat = lat;
  211. packet.lon = lon;
  212. packet.alt = alt;
  213. packet.relative_alt = relative_alt;
  214. packet.next_lat = next_lat;
  215. packet.next_lon = next_lon;
  216. packet.next_alt = next_alt;
  217. packet.vx = vx;
  218. packet.vy = vy;
  219. packet.vz = vz;
  220. packet.h_acc = h_acc;
  221. packet.v_acc = v_acc;
  222. packet.vel_acc = vel_acc;
  223. packet.update_rate = update_rate;
  224. packet.flight_state = flight_state;
  225. packet.flags = flags;
  226. mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18);
  227. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  228. #endif
  229. msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION;
  230. #if MAVLINK_CRC_EXTRA
  231. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  232. #else
  233. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  234. #endif
  235. }
  236. /**
  237. * @brief Pack a utm_global_position message on a channel
  238. * @param system_id ID of this system
  239. * @param component_id ID of this component (e.g. 200 for IMU)
  240. * @param chan The MAVLink channel this message will be sent over
  241. * @param msg The MAVLink message to compress the data into
  242. * @param time [us] Time of applicability of position (microseconds since UNIX epoch).
  243. * @param uas_id Unique UAS ID.
  244. * @param lat [degE7] Latitude (WGS84)
  245. * @param lon [degE7] Longitude (WGS84)
  246. * @param alt [mm] Altitude (WGS84)
  247. * @param relative_alt [mm] Altitude above ground
  248. * @param vx [cm/s] Ground X speed (latitude, positive north)
  249. * @param vy [cm/s] Ground Y speed (longitude, positive east)
  250. * @param vz [cm/s] Ground Z speed (altitude, positive down)
  251. * @param h_acc [mm] Horizontal position uncertainty (standard deviation)
  252. * @param v_acc [mm] Altitude uncertainty (standard deviation)
  253. * @param vel_acc [cm/s] Speed uncertainty (standard deviation)
  254. * @param next_lat [degE7] Next waypoint, latitude (WGS84)
  255. * @param next_lon [degE7] Next waypoint, longitude (WGS84)
  256. * @param next_alt [mm] Next waypoint, altitude (WGS84)
  257. * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode.
  258. * @param flight_state Flight state
  259. * @param flags Bitwise OR combination of the data available flags.
  260. * @return length of the message in bytes (excluding serial stream start sign)
  261. */
  262. static inline uint16_t mavlink_msg_utm_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  263. mavlink_message_t* msg,
  264. uint64_t time,const uint8_t *uas_id,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,int16_t vx,int16_t vy,int16_t vz,uint16_t h_acc,uint16_t v_acc,uint16_t vel_acc,int32_t next_lat,int32_t next_lon,int32_t next_alt,uint16_t update_rate,uint8_t flight_state,uint8_t flags)
  265. {
  266. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  267. char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN];
  268. _mav_put_uint64_t(buf, 0, time);
  269. _mav_put_int32_t(buf, 8, lat);
  270. _mav_put_int32_t(buf, 12, lon);
  271. _mav_put_int32_t(buf, 16, alt);
  272. _mav_put_int32_t(buf, 20, relative_alt);
  273. _mav_put_int32_t(buf, 24, next_lat);
  274. _mav_put_int32_t(buf, 28, next_lon);
  275. _mav_put_int32_t(buf, 32, next_alt);
  276. _mav_put_int16_t(buf, 36, vx);
  277. _mav_put_int16_t(buf, 38, vy);
  278. _mav_put_int16_t(buf, 40, vz);
  279. _mav_put_uint16_t(buf, 42, h_acc);
  280. _mav_put_uint16_t(buf, 44, v_acc);
  281. _mav_put_uint16_t(buf, 46, vel_acc);
  282. _mav_put_uint16_t(buf, 48, update_rate);
  283. _mav_put_uint8_t(buf, 68, flight_state);
  284. _mav_put_uint8_t(buf, 69, flags);
  285. _mav_put_uint8_t_array(buf, 50, uas_id, 18);
  286. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  287. #else
  288. mavlink_utm_global_position_t packet;
  289. packet.time = time;
  290. packet.lat = lat;
  291. packet.lon = lon;
  292. packet.alt = alt;
  293. packet.relative_alt = relative_alt;
  294. packet.next_lat = next_lat;
  295. packet.next_lon = next_lon;
  296. packet.next_alt = next_alt;
  297. packet.vx = vx;
  298. packet.vy = vy;
  299. packet.vz = vz;
  300. packet.h_acc = h_acc;
  301. packet.v_acc = v_acc;
  302. packet.vel_acc = vel_acc;
  303. packet.update_rate = update_rate;
  304. packet.flight_state = flight_state;
  305. packet.flags = flags;
  306. mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18);
  307. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  308. #endif
  309. msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION;
  310. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  311. }
  312. /**
  313. * @brief Encode a utm_global_position struct
  314. *
  315. * @param system_id ID of this system
  316. * @param component_id ID of this component (e.g. 200 for IMU)
  317. * @param msg The MAVLink message to compress the data into
  318. * @param utm_global_position C-struct to read the message contents from
  319. */
  320. static inline uint16_t mavlink_msg_utm_global_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position)
  321. {
  322. return mavlink_msg_utm_global_position_pack(system_id, component_id, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags);
  323. }
  324. /**
  325. * @brief Encode a utm_global_position struct on a channel
  326. *
  327. * @param system_id ID of this system
  328. * @param component_id ID of this component (e.g. 200 for IMU)
  329. * @param chan The MAVLink channel this message will be sent over
  330. * @param msg The MAVLink message to compress the data into
  331. * @param utm_global_position C-struct to read the message contents from
  332. */
  333. static inline uint16_t mavlink_msg_utm_global_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position)
  334. {
  335. return mavlink_msg_utm_global_position_pack_chan(system_id, component_id, chan, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags);
  336. }
  337. /**
  338. * @brief Encode a utm_global_position struct with provided status structure
  339. *
  340. * @param system_id ID of this system
  341. * @param component_id ID of this component (e.g. 200 for IMU)
  342. * @param status MAVLink status structure
  343. * @param msg The MAVLink message to compress the data into
  344. * @param utm_global_position C-struct to read the message contents from
  345. */
  346. static inline uint16_t mavlink_msg_utm_global_position_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position)
  347. {
  348. return mavlink_msg_utm_global_position_pack_status(system_id, component_id, _status, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags);
  349. }
  350. /**
  351. * @brief Send a utm_global_position message
  352. * @param chan MAVLink channel to send the message
  353. *
  354. * @param time [us] Time of applicability of position (microseconds since UNIX epoch).
  355. * @param uas_id Unique UAS ID.
  356. * @param lat [degE7] Latitude (WGS84)
  357. * @param lon [degE7] Longitude (WGS84)
  358. * @param alt [mm] Altitude (WGS84)
  359. * @param relative_alt [mm] Altitude above ground
  360. * @param vx [cm/s] Ground X speed (latitude, positive north)
  361. * @param vy [cm/s] Ground Y speed (longitude, positive east)
  362. * @param vz [cm/s] Ground Z speed (altitude, positive down)
  363. * @param h_acc [mm] Horizontal position uncertainty (standard deviation)
  364. * @param v_acc [mm] Altitude uncertainty (standard deviation)
  365. * @param vel_acc [cm/s] Speed uncertainty (standard deviation)
  366. * @param next_lat [degE7] Next waypoint, latitude (WGS84)
  367. * @param next_lon [degE7] Next waypoint, longitude (WGS84)
  368. * @param next_alt [mm] Next waypoint, altitude (WGS84)
  369. * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode.
  370. * @param flight_state Flight state
  371. * @param flags Bitwise OR combination of the data available flags.
  372. */
  373. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  374. static inline void mavlink_msg_utm_global_position_send(mavlink_channel_t chan, uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags)
  375. {
  376. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  377. char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN];
  378. _mav_put_uint64_t(buf, 0, time);
  379. _mav_put_int32_t(buf, 8, lat);
  380. _mav_put_int32_t(buf, 12, lon);
  381. _mav_put_int32_t(buf, 16, alt);
  382. _mav_put_int32_t(buf, 20, relative_alt);
  383. _mav_put_int32_t(buf, 24, next_lat);
  384. _mav_put_int32_t(buf, 28, next_lon);
  385. _mav_put_int32_t(buf, 32, next_alt);
  386. _mav_put_int16_t(buf, 36, vx);
  387. _mav_put_int16_t(buf, 38, vy);
  388. _mav_put_int16_t(buf, 40, vz);
  389. _mav_put_uint16_t(buf, 42, h_acc);
  390. _mav_put_uint16_t(buf, 44, v_acc);
  391. _mav_put_uint16_t(buf, 46, vel_acc);
  392. _mav_put_uint16_t(buf, 48, update_rate);
  393. _mav_put_uint8_t(buf, 68, flight_state);
  394. _mav_put_uint8_t(buf, 69, flags);
  395. _mav_put_uint8_t_array(buf, 50, uas_id, 18);
  396. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  397. #else
  398. mavlink_utm_global_position_t packet;
  399. packet.time = time;
  400. packet.lat = lat;
  401. packet.lon = lon;
  402. packet.alt = alt;
  403. packet.relative_alt = relative_alt;
  404. packet.next_lat = next_lat;
  405. packet.next_lon = next_lon;
  406. packet.next_alt = next_alt;
  407. packet.vx = vx;
  408. packet.vy = vy;
  409. packet.vz = vz;
  410. packet.h_acc = h_acc;
  411. packet.v_acc = v_acc;
  412. packet.vel_acc = vel_acc;
  413. packet.update_rate = update_rate;
  414. packet.flight_state = flight_state;
  415. packet.flags = flags;
  416. mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18);
  417. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)&packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  418. #endif
  419. }
  420. /**
  421. * @brief Send a utm_global_position message
  422. * @param chan MAVLink channel to send the message
  423. * @param struct The MAVLink struct to serialize
  424. */
  425. static inline void mavlink_msg_utm_global_position_send_struct(mavlink_channel_t chan, const mavlink_utm_global_position_t* utm_global_position)
  426. {
  427. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  428. mavlink_msg_utm_global_position_send(chan, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags);
  429. #else
  430. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)utm_global_position, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  431. #endif
  432. }
  433. #if MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  434. /*
  435. This variant of _send() can be used to save stack space by re-using
  436. memory from the receive buffer. The caller provides a
  437. mavlink_message_t which is the size of a full mavlink message. This
  438. is usually the receive buffer for the channel, and allows a reply to an
  439. incoming message with minimum stack space usage.
  440. */
  441. static inline void mavlink_msg_utm_global_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags)
  442. {
  443. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  444. char *buf = (char *)msgbuf;
  445. _mav_put_uint64_t(buf, 0, time);
  446. _mav_put_int32_t(buf, 8, lat);
  447. _mav_put_int32_t(buf, 12, lon);
  448. _mav_put_int32_t(buf, 16, alt);
  449. _mav_put_int32_t(buf, 20, relative_alt);
  450. _mav_put_int32_t(buf, 24, next_lat);
  451. _mav_put_int32_t(buf, 28, next_lon);
  452. _mav_put_int32_t(buf, 32, next_alt);
  453. _mav_put_int16_t(buf, 36, vx);
  454. _mav_put_int16_t(buf, 38, vy);
  455. _mav_put_int16_t(buf, 40, vz);
  456. _mav_put_uint16_t(buf, 42, h_acc);
  457. _mav_put_uint16_t(buf, 44, v_acc);
  458. _mav_put_uint16_t(buf, 46, vel_acc);
  459. _mav_put_uint16_t(buf, 48, update_rate);
  460. _mav_put_uint8_t(buf, 68, flight_state);
  461. _mav_put_uint8_t(buf, 69, flags);
  462. _mav_put_uint8_t_array(buf, 50, uas_id, 18);
  463. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  464. #else
  465. mavlink_utm_global_position_t *packet = (mavlink_utm_global_position_t *)msgbuf;
  466. packet->time = time;
  467. packet->lat = lat;
  468. packet->lon = lon;
  469. packet->alt = alt;
  470. packet->relative_alt = relative_alt;
  471. packet->next_lat = next_lat;
  472. packet->next_lon = next_lon;
  473. packet->next_alt = next_alt;
  474. packet->vx = vx;
  475. packet->vy = vy;
  476. packet->vz = vz;
  477. packet->h_acc = h_acc;
  478. packet->v_acc = v_acc;
  479. packet->vel_acc = vel_acc;
  480. packet->update_rate = update_rate;
  481. packet->flight_state = flight_state;
  482. packet->flags = flags;
  483. mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*18);
  484. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC);
  485. #endif
  486. }
  487. #endif
  488. #endif
  489. // MESSAGE UTM_GLOBAL_POSITION UNPACKING
  490. /**
  491. * @brief Get field time from utm_global_position message
  492. *
  493. * @return [us] Time of applicability of position (microseconds since UNIX epoch).
  494. */
  495. static inline uint64_t mavlink_msg_utm_global_position_get_time(const mavlink_message_t* msg)
  496. {
  497. return _MAV_RETURN_uint64_t(msg, 0);
  498. }
  499. /**
  500. * @brief Get field uas_id from utm_global_position message
  501. *
  502. * @return Unique UAS ID.
  503. */
  504. static inline uint16_t mavlink_msg_utm_global_position_get_uas_id(const mavlink_message_t* msg, uint8_t *uas_id)
  505. {
  506. return _MAV_RETURN_uint8_t_array(msg, uas_id, 18, 50);
  507. }
  508. /**
  509. * @brief Get field lat from utm_global_position message
  510. *
  511. * @return [degE7] Latitude (WGS84)
  512. */
  513. static inline int32_t mavlink_msg_utm_global_position_get_lat(const mavlink_message_t* msg)
  514. {
  515. return _MAV_RETURN_int32_t(msg, 8);
  516. }
  517. /**
  518. * @brief Get field lon from utm_global_position message
  519. *
  520. * @return [degE7] Longitude (WGS84)
  521. */
  522. static inline int32_t mavlink_msg_utm_global_position_get_lon(const mavlink_message_t* msg)
  523. {
  524. return _MAV_RETURN_int32_t(msg, 12);
  525. }
  526. /**
  527. * @brief Get field alt from utm_global_position message
  528. *
  529. * @return [mm] Altitude (WGS84)
  530. */
  531. static inline int32_t mavlink_msg_utm_global_position_get_alt(const mavlink_message_t* msg)
  532. {
  533. return _MAV_RETURN_int32_t(msg, 16);
  534. }
  535. /**
  536. * @brief Get field relative_alt from utm_global_position message
  537. *
  538. * @return [mm] Altitude above ground
  539. */
  540. static inline int32_t mavlink_msg_utm_global_position_get_relative_alt(const mavlink_message_t* msg)
  541. {
  542. return _MAV_RETURN_int32_t(msg, 20);
  543. }
  544. /**
  545. * @brief Get field vx from utm_global_position message
  546. *
  547. * @return [cm/s] Ground X speed (latitude, positive north)
  548. */
  549. static inline int16_t mavlink_msg_utm_global_position_get_vx(const mavlink_message_t* msg)
  550. {
  551. return _MAV_RETURN_int16_t(msg, 36);
  552. }
  553. /**
  554. * @brief Get field vy from utm_global_position message
  555. *
  556. * @return [cm/s] Ground Y speed (longitude, positive east)
  557. */
  558. static inline int16_t mavlink_msg_utm_global_position_get_vy(const mavlink_message_t* msg)
  559. {
  560. return _MAV_RETURN_int16_t(msg, 38);
  561. }
  562. /**
  563. * @brief Get field vz from utm_global_position message
  564. *
  565. * @return [cm/s] Ground Z speed (altitude, positive down)
  566. */
  567. static inline int16_t mavlink_msg_utm_global_position_get_vz(const mavlink_message_t* msg)
  568. {
  569. return _MAV_RETURN_int16_t(msg, 40);
  570. }
  571. /**
  572. * @brief Get field h_acc from utm_global_position message
  573. *
  574. * @return [mm] Horizontal position uncertainty (standard deviation)
  575. */
  576. static inline uint16_t mavlink_msg_utm_global_position_get_h_acc(const mavlink_message_t* msg)
  577. {
  578. return _MAV_RETURN_uint16_t(msg, 42);
  579. }
  580. /**
  581. * @brief Get field v_acc from utm_global_position message
  582. *
  583. * @return [mm] Altitude uncertainty (standard deviation)
  584. */
  585. static inline uint16_t mavlink_msg_utm_global_position_get_v_acc(const mavlink_message_t* msg)
  586. {
  587. return _MAV_RETURN_uint16_t(msg, 44);
  588. }
  589. /**
  590. * @brief Get field vel_acc from utm_global_position message
  591. *
  592. * @return [cm/s] Speed uncertainty (standard deviation)
  593. */
  594. static inline uint16_t mavlink_msg_utm_global_position_get_vel_acc(const mavlink_message_t* msg)
  595. {
  596. return _MAV_RETURN_uint16_t(msg, 46);
  597. }
  598. /**
  599. * @brief Get field next_lat from utm_global_position message
  600. *
  601. * @return [degE7] Next waypoint, latitude (WGS84)
  602. */
  603. static inline int32_t mavlink_msg_utm_global_position_get_next_lat(const mavlink_message_t* msg)
  604. {
  605. return _MAV_RETURN_int32_t(msg, 24);
  606. }
  607. /**
  608. * @brief Get field next_lon from utm_global_position message
  609. *
  610. * @return [degE7] Next waypoint, longitude (WGS84)
  611. */
  612. static inline int32_t mavlink_msg_utm_global_position_get_next_lon(const mavlink_message_t* msg)
  613. {
  614. return _MAV_RETURN_int32_t(msg, 28);
  615. }
  616. /**
  617. * @brief Get field next_alt from utm_global_position message
  618. *
  619. * @return [mm] Next waypoint, altitude (WGS84)
  620. */
  621. static inline int32_t mavlink_msg_utm_global_position_get_next_alt(const mavlink_message_t* msg)
  622. {
  623. return _MAV_RETURN_int32_t(msg, 32);
  624. }
  625. /**
  626. * @brief Get field update_rate from utm_global_position message
  627. *
  628. * @return [cs] Time until next update. Set to 0 if unknown or in data driven mode.
  629. */
  630. static inline uint16_t mavlink_msg_utm_global_position_get_update_rate(const mavlink_message_t* msg)
  631. {
  632. return _MAV_RETURN_uint16_t(msg, 48);
  633. }
  634. /**
  635. * @brief Get field flight_state from utm_global_position message
  636. *
  637. * @return Flight state
  638. */
  639. static inline uint8_t mavlink_msg_utm_global_position_get_flight_state(const mavlink_message_t* msg)
  640. {
  641. return _MAV_RETURN_uint8_t(msg, 68);
  642. }
  643. /**
  644. * @brief Get field flags from utm_global_position message
  645. *
  646. * @return Bitwise OR combination of the data available flags.
  647. */
  648. static inline uint8_t mavlink_msg_utm_global_position_get_flags(const mavlink_message_t* msg)
  649. {
  650. return _MAV_RETURN_uint8_t(msg, 69);
  651. }
  652. /**
  653. * @brief Decode a utm_global_position message into a struct
  654. *
  655. * @param msg The message to decode
  656. * @param utm_global_position C-struct to decode the message contents into
  657. */
  658. static inline void mavlink_msg_utm_global_position_decode(const mavlink_message_t* msg, mavlink_utm_global_position_t* utm_global_position)
  659. {
  660. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  661. utm_global_position->time = mavlink_msg_utm_global_position_get_time(msg);
  662. utm_global_position->lat = mavlink_msg_utm_global_position_get_lat(msg);
  663. utm_global_position->lon = mavlink_msg_utm_global_position_get_lon(msg);
  664. utm_global_position->alt = mavlink_msg_utm_global_position_get_alt(msg);
  665. utm_global_position->relative_alt = mavlink_msg_utm_global_position_get_relative_alt(msg);
  666. utm_global_position->next_lat = mavlink_msg_utm_global_position_get_next_lat(msg);
  667. utm_global_position->next_lon = mavlink_msg_utm_global_position_get_next_lon(msg);
  668. utm_global_position->next_alt = mavlink_msg_utm_global_position_get_next_alt(msg);
  669. utm_global_position->vx = mavlink_msg_utm_global_position_get_vx(msg);
  670. utm_global_position->vy = mavlink_msg_utm_global_position_get_vy(msg);
  671. utm_global_position->vz = mavlink_msg_utm_global_position_get_vz(msg);
  672. utm_global_position->h_acc = mavlink_msg_utm_global_position_get_h_acc(msg);
  673. utm_global_position->v_acc = mavlink_msg_utm_global_position_get_v_acc(msg);
  674. utm_global_position->vel_acc = mavlink_msg_utm_global_position_get_vel_acc(msg);
  675. utm_global_position->update_rate = mavlink_msg_utm_global_position_get_update_rate(msg);
  676. mavlink_msg_utm_global_position_get_uas_id(msg, utm_global_position->uas_id);
  677. utm_global_position->flight_state = mavlink_msg_utm_global_position_get_flight_state(msg);
  678. utm_global_position->flags = mavlink_msg_utm_global_position_get_flags(msg);
  679. #else
  680. uint8_t len = msg->len < MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN? msg->len : MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN;
  681. memset(utm_global_position, 0, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN);
  682. memcpy(utm_global_position, _MAV_PAYLOAD(msg), len);
  683. #endif
  684. }