mavlink_msg_terrain_data.h 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362
  1. #pragma once
  2. // MESSAGE TERRAIN_DATA PACKING
  3. #define MAVLINK_MSG_ID_TERRAIN_DATA 134
  4. typedef struct __mavlink_terrain_data_t {
  5. int32_t lat; /*< [degE7] Latitude of SW corner of first grid*/
  6. int32_t lon; /*< [degE7] Longitude of SW corner of first grid*/
  7. uint16_t grid_spacing; /*< [m] Grid spacing*/
  8. int16_t data[16]; /*< [m] Terrain data MSL*/
  9. uint8_t gridbit; /*< bit within the terrain request mask*/
  10. } mavlink_terrain_data_t;
  11. #define MAVLINK_MSG_ID_TERRAIN_DATA_LEN 43
  12. #define MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN 43
  13. #define MAVLINK_MSG_ID_134_LEN 43
  14. #define MAVLINK_MSG_ID_134_MIN_LEN 43
  15. #define MAVLINK_MSG_ID_TERRAIN_DATA_CRC 229
  16. #define MAVLINK_MSG_ID_134_CRC 229
  17. #define MAVLINK_MSG_TERRAIN_DATA_FIELD_DATA_LEN 16
  18. #if MAVLINK_COMMAND_24BIT
  19. #define MAVLINK_MESSAGE_INFO_TERRAIN_DATA { \
  20. 134, \
  21. "TERRAIN_DATA", \
  22. 5, \
  23. { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_data_t, lat) }, \
  24. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_data_t, lon) }, \
  25. { "grid_spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_terrain_data_t, grid_spacing) }, \
  26. { "gridbit", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_terrain_data_t, gridbit) }, \
  27. { "data", NULL, MAVLINK_TYPE_INT16_T, 16, 10, offsetof(mavlink_terrain_data_t, data) }, \
  28. } \
  29. }
  30. #else
  31. #define MAVLINK_MESSAGE_INFO_TERRAIN_DATA { \
  32. "TERRAIN_DATA", \
  33. 5, \
  34. { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_terrain_data_t, lat) }, \
  35. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_terrain_data_t, lon) }, \
  36. { "grid_spacing", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_terrain_data_t, grid_spacing) }, \
  37. { "gridbit", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_terrain_data_t, gridbit) }, \
  38. { "data", NULL, MAVLINK_TYPE_INT16_T, 16, 10, offsetof(mavlink_terrain_data_t, data) }, \
  39. } \
  40. }
  41. #endif
  42. /**
  43. * @brief Pack a terrain_data message
  44. * @param system_id ID of this system
  45. * @param component_id ID of this component (e.g. 200 for IMU)
  46. * @param msg The MAVLink message to compress the data into
  47. *
  48. * @param lat [degE7] Latitude of SW corner of first grid
  49. * @param lon [degE7] Longitude of SW corner of first grid
  50. * @param grid_spacing [m] Grid spacing
  51. * @param gridbit bit within the terrain request mask
  52. * @param data [m] Terrain data MSL
  53. * @return length of the message in bytes (excluding serial stream start sign)
  54. */
  55. static inline uint16_t mavlink_msg_terrain_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  56. int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data)
  57. {
  58. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  59. char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN];
  60. _mav_put_int32_t(buf, 0, lat);
  61. _mav_put_int32_t(buf, 4, lon);
  62. _mav_put_uint16_t(buf, 8, grid_spacing);
  63. _mav_put_uint8_t(buf, 42, gridbit);
  64. _mav_put_int16_t_array(buf, 10, data, 16);
  65. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  66. #else
  67. mavlink_terrain_data_t packet;
  68. packet.lat = lat;
  69. packet.lon = lon;
  70. packet.grid_spacing = grid_spacing;
  71. packet.gridbit = gridbit;
  72. mav_array_memcpy(packet.data, data, sizeof(int16_t)*16);
  73. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  74. #endif
  75. msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA;
  76. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  77. }
  78. /**
  79. * @brief Pack a terrain_data message
  80. * @param system_id ID of this system
  81. * @param component_id ID of this component (e.g. 200 for IMU)
  82. * @param status MAVLink status structure
  83. * @param msg The MAVLink message to compress the data into
  84. *
  85. * @param lat [degE7] Latitude of SW corner of first grid
  86. * @param lon [degE7] Longitude of SW corner of first grid
  87. * @param grid_spacing [m] Grid spacing
  88. * @param gridbit bit within the terrain request mask
  89. * @param data [m] Terrain data MSL
  90. * @return length of the message in bytes (excluding serial stream start sign)
  91. */
  92. static inline uint16_t mavlink_msg_terrain_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
  93. int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data)
  94. {
  95. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  96. char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN];
  97. _mav_put_int32_t(buf, 0, lat);
  98. _mav_put_int32_t(buf, 4, lon);
  99. _mav_put_uint16_t(buf, 8, grid_spacing);
  100. _mav_put_uint8_t(buf, 42, gridbit);
  101. _mav_put_int16_t_array(buf, 10, data, 16);
  102. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  103. #else
  104. mavlink_terrain_data_t packet;
  105. packet.lat = lat;
  106. packet.lon = lon;
  107. packet.grid_spacing = grid_spacing;
  108. packet.gridbit = gridbit;
  109. mav_array_memcpy(packet.data, data, sizeof(int16_t)*16);
  110. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  111. #endif
  112. msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA;
  113. #if MAVLINK_CRC_EXTRA
  114. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  115. #else
  116. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  117. #endif
  118. }
  119. /**
  120. * @brief Pack a terrain_data message on a channel
  121. * @param system_id ID of this system
  122. * @param component_id ID of this component (e.g. 200 for IMU)
  123. * @param chan The MAVLink channel this message will be sent over
  124. * @param msg The MAVLink message to compress the data into
  125. * @param lat [degE7] Latitude of SW corner of first grid
  126. * @param lon [degE7] Longitude of SW corner of first grid
  127. * @param grid_spacing [m] Grid spacing
  128. * @param gridbit bit within the terrain request mask
  129. * @param data [m] Terrain data MSL
  130. * @return length of the message in bytes (excluding serial stream start sign)
  131. */
  132. static inline uint16_t mavlink_msg_terrain_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  133. mavlink_message_t* msg,
  134. int32_t lat,int32_t lon,uint16_t grid_spacing,uint8_t gridbit,const int16_t *data)
  135. {
  136. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  137. char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN];
  138. _mav_put_int32_t(buf, 0, lat);
  139. _mav_put_int32_t(buf, 4, lon);
  140. _mav_put_uint16_t(buf, 8, grid_spacing);
  141. _mav_put_uint8_t(buf, 42, gridbit);
  142. _mav_put_int16_t_array(buf, 10, data, 16);
  143. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  144. #else
  145. mavlink_terrain_data_t packet;
  146. packet.lat = lat;
  147. packet.lon = lon;
  148. packet.grid_spacing = grid_spacing;
  149. packet.gridbit = gridbit;
  150. mav_array_memcpy(packet.data, data, sizeof(int16_t)*16);
  151. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  152. #endif
  153. msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA;
  154. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  155. }
  156. /**
  157. * @brief Encode a terrain_data struct
  158. *
  159. * @param system_id ID of this system
  160. * @param component_id ID of this component (e.g. 200 for IMU)
  161. * @param msg The MAVLink message to compress the data into
  162. * @param terrain_data C-struct to read the message contents from
  163. */
  164. static inline uint16_t mavlink_msg_terrain_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data)
  165. {
  166. return mavlink_msg_terrain_data_pack(system_id, component_id, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data);
  167. }
  168. /**
  169. * @brief Encode a terrain_data struct on a channel
  170. *
  171. * @param system_id ID of this system
  172. * @param component_id ID of this component (e.g. 200 for IMU)
  173. * @param chan The MAVLink channel this message will be sent over
  174. * @param msg The MAVLink message to compress the data into
  175. * @param terrain_data C-struct to read the message contents from
  176. */
  177. static inline uint16_t mavlink_msg_terrain_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data)
  178. {
  179. return mavlink_msg_terrain_data_pack_chan(system_id, component_id, chan, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data);
  180. }
  181. /**
  182. * @brief Encode a terrain_data struct with provided status structure
  183. *
  184. * @param system_id ID of this system
  185. * @param component_id ID of this component (e.g. 200 for IMU)
  186. * @param status MAVLink status structure
  187. * @param msg The MAVLink message to compress the data into
  188. * @param terrain_data C-struct to read the message contents from
  189. */
  190. static inline uint16_t mavlink_msg_terrain_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data)
  191. {
  192. return mavlink_msg_terrain_data_pack_status(system_id, component_id, _status, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data);
  193. }
  194. /**
  195. * @brief Send a terrain_data message
  196. * @param chan MAVLink channel to send the message
  197. *
  198. * @param lat [degE7] Latitude of SW corner of first grid
  199. * @param lon [degE7] Longitude of SW corner of first grid
  200. * @param grid_spacing [m] Grid spacing
  201. * @param gridbit bit within the terrain request mask
  202. * @param data [m] Terrain data MSL
  203. */
  204. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  205. static inline void mavlink_msg_terrain_data_send(mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data)
  206. {
  207. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  208. char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN];
  209. _mav_put_int32_t(buf, 0, lat);
  210. _mav_put_int32_t(buf, 4, lon);
  211. _mav_put_uint16_t(buf, 8, grid_spacing);
  212. _mav_put_uint8_t(buf, 42, gridbit);
  213. _mav_put_int16_t_array(buf, 10, data, 16);
  214. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  215. #else
  216. mavlink_terrain_data_t packet;
  217. packet.lat = lat;
  218. packet.lon = lon;
  219. packet.grid_spacing = grid_spacing;
  220. packet.gridbit = gridbit;
  221. mav_array_memcpy(packet.data, data, sizeof(int16_t)*16);
  222. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  223. #endif
  224. }
  225. /**
  226. * @brief Send a terrain_data message
  227. * @param chan MAVLink channel to send the message
  228. * @param struct The MAVLink struct to serialize
  229. */
  230. static inline void mavlink_msg_terrain_data_send_struct(mavlink_channel_t chan, const mavlink_terrain_data_t* terrain_data)
  231. {
  232. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  233. mavlink_msg_terrain_data_send(chan, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data);
  234. #else
  235. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)terrain_data, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  236. #endif
  237. }
  238. #if MAVLINK_MSG_ID_TERRAIN_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  239. /*
  240. This variant of _send() can be used to save stack space by re-using
  241. memory from the receive buffer. The caller provides a
  242. mavlink_message_t which is the size of a full mavlink message. This
  243. is usually the receive buffer for the channel, and allows a reply to an
  244. incoming message with minimum stack space usage.
  245. */
  246. static inline void mavlink_msg_terrain_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data)
  247. {
  248. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  249. char *buf = (char *)msgbuf;
  250. _mav_put_int32_t(buf, 0, lat);
  251. _mav_put_int32_t(buf, 4, lon);
  252. _mav_put_uint16_t(buf, 8, grid_spacing);
  253. _mav_put_uint8_t(buf, 42, gridbit);
  254. _mav_put_int16_t_array(buf, 10, data, 16);
  255. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, buf, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  256. #else
  257. mavlink_terrain_data_t *packet = (mavlink_terrain_data_t *)msgbuf;
  258. packet->lat = lat;
  259. packet->lon = lon;
  260. packet->grid_spacing = grid_spacing;
  261. packet->gridbit = gridbit;
  262. mav_array_memcpy(packet->data, data, sizeof(int16_t)*16);
  263. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC);
  264. #endif
  265. }
  266. #endif
  267. #endif
  268. // MESSAGE TERRAIN_DATA UNPACKING
  269. /**
  270. * @brief Get field lat from terrain_data message
  271. *
  272. * @return [degE7] Latitude of SW corner of first grid
  273. */
  274. static inline int32_t mavlink_msg_terrain_data_get_lat(const mavlink_message_t* msg)
  275. {
  276. return _MAV_RETURN_int32_t(msg, 0);
  277. }
  278. /**
  279. * @brief Get field lon from terrain_data message
  280. *
  281. * @return [degE7] Longitude of SW corner of first grid
  282. */
  283. static inline int32_t mavlink_msg_terrain_data_get_lon(const mavlink_message_t* msg)
  284. {
  285. return _MAV_RETURN_int32_t(msg, 4);
  286. }
  287. /**
  288. * @brief Get field grid_spacing from terrain_data message
  289. *
  290. * @return [m] Grid spacing
  291. */
  292. static inline uint16_t mavlink_msg_terrain_data_get_grid_spacing(const mavlink_message_t* msg)
  293. {
  294. return _MAV_RETURN_uint16_t(msg, 8);
  295. }
  296. /**
  297. * @brief Get field gridbit from terrain_data message
  298. *
  299. * @return bit within the terrain request mask
  300. */
  301. static inline uint8_t mavlink_msg_terrain_data_get_gridbit(const mavlink_message_t* msg)
  302. {
  303. return _MAV_RETURN_uint8_t(msg, 42);
  304. }
  305. /**
  306. * @brief Get field data from terrain_data message
  307. *
  308. * @return [m] Terrain data MSL
  309. */
  310. static inline uint16_t mavlink_msg_terrain_data_get_data(const mavlink_message_t* msg, int16_t *data)
  311. {
  312. return _MAV_RETURN_int16_t_array(msg, data, 16, 10);
  313. }
  314. /**
  315. * @brief Decode a terrain_data message into a struct
  316. *
  317. * @param msg The message to decode
  318. * @param terrain_data C-struct to decode the message contents into
  319. */
  320. static inline void mavlink_msg_terrain_data_decode(const mavlink_message_t* msg, mavlink_terrain_data_t* terrain_data)
  321. {
  322. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  323. terrain_data->lat = mavlink_msg_terrain_data_get_lat(msg);
  324. terrain_data->lon = mavlink_msg_terrain_data_get_lon(msg);
  325. terrain_data->grid_spacing = mavlink_msg_terrain_data_get_grid_spacing(msg);
  326. mavlink_msg_terrain_data_get_data(msg, terrain_data->data);
  327. terrain_data->gridbit = mavlink_msg_terrain_data_get_gridbit(msg);
  328. #else
  329. uint8_t len = msg->len < MAVLINK_MSG_ID_TERRAIN_DATA_LEN? msg->len : MAVLINK_MSG_ID_TERRAIN_DATA_LEN;
  330. memset(terrain_data, 0, MAVLINK_MSG_ID_TERRAIN_DATA_LEN);
  331. memcpy(terrain_data, _MAV_PAYLOAD(msg), len);
  332. #endif
  333. }