mavlink_msg_ping.h 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344
  1. #pragma once
  2. // MESSAGE PING PACKING
  3. #define MAVLINK_MSG_ID_PING 4
  4. typedef struct __mavlink_ping_t {
  5. uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/
  6. uint32_t seq; /*< PING sequence*/
  7. uint8_t target_system; /*< 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system*/
  8. uint8_t target_component; /*< 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component.*/
  9. } mavlink_ping_t;
  10. #define MAVLINK_MSG_ID_PING_LEN 14
  11. #define MAVLINK_MSG_ID_PING_MIN_LEN 14
  12. #define MAVLINK_MSG_ID_4_LEN 14
  13. #define MAVLINK_MSG_ID_4_MIN_LEN 14
  14. #define MAVLINK_MSG_ID_PING_CRC 237
  15. #define MAVLINK_MSG_ID_4_CRC 237
  16. #if MAVLINK_COMMAND_24BIT
  17. #define MAVLINK_MESSAGE_INFO_PING { \
  18. 4, \
  19. "PING", \
  20. 4, \
  21. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_ping_t, time_usec) }, \
  22. { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ping_t, seq) }, \
  23. { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_ping_t, target_system) }, \
  24. { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_ping_t, target_component) }, \
  25. } \
  26. }
  27. #else
  28. #define MAVLINK_MESSAGE_INFO_PING { \
  29. "PING", \
  30. 4, \
  31. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_ping_t, time_usec) }, \
  32. { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_ping_t, seq) }, \
  33. { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_ping_t, target_system) }, \
  34. { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_ping_t, target_component) }, \
  35. } \
  36. }
  37. #endif
  38. /**
  39. * @brief Pack a ping message
  40. * @param system_id ID of this system
  41. * @param component_id ID of this component (e.g. 200 for IMU)
  42. * @param msg The MAVLink message to compress the data into
  43. *
  44. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  45. * @param seq PING sequence
  46. * @param target_system 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system
  47. * @param target_component 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component.
  48. * @return length of the message in bytes (excluding serial stream start sign)
  49. */
  50. static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  51. uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component)
  52. {
  53. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  54. char buf[MAVLINK_MSG_ID_PING_LEN];
  55. _mav_put_uint64_t(buf, 0, time_usec);
  56. _mav_put_uint32_t(buf, 8, seq);
  57. _mav_put_uint8_t(buf, 12, target_system);
  58. _mav_put_uint8_t(buf, 13, target_component);
  59. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
  60. #else
  61. mavlink_ping_t packet;
  62. packet.time_usec = time_usec;
  63. packet.seq = seq;
  64. packet.target_system = target_system;
  65. packet.target_component = target_component;
  66. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
  67. #endif
  68. msg->msgid = MAVLINK_MSG_ID_PING;
  69. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  70. }
  71. /**
  72. * @brief Pack a ping message
  73. * @param system_id ID of this system
  74. * @param component_id ID of this component (e.g. 200 for IMU)
  75. * @param status MAVLink status structure
  76. * @param msg The MAVLink message to compress the data into
  77. *
  78. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  79. * @param seq PING sequence
  80. * @param target_system 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system
  81. * @param target_component 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component.
  82. * @return length of the message in bytes (excluding serial stream start sign)
  83. */
  84. static inline uint16_t mavlink_msg_ping_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
  85. uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component)
  86. {
  87. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  88. char buf[MAVLINK_MSG_ID_PING_LEN];
  89. _mav_put_uint64_t(buf, 0, time_usec);
  90. _mav_put_uint32_t(buf, 8, seq);
  91. _mav_put_uint8_t(buf, 12, target_system);
  92. _mav_put_uint8_t(buf, 13, target_component);
  93. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
  94. #else
  95. mavlink_ping_t packet;
  96. packet.time_usec = time_usec;
  97. packet.seq = seq;
  98. packet.target_system = target_system;
  99. packet.target_component = target_component;
  100. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
  101. #endif
  102. msg->msgid = MAVLINK_MSG_ID_PING;
  103. #if MAVLINK_CRC_EXTRA
  104. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  105. #else
  106. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN);
  107. #endif
  108. }
  109. /**
  110. * @brief Pack a ping message on a channel
  111. * @param system_id ID of this system
  112. * @param component_id ID of this component (e.g. 200 for IMU)
  113. * @param chan The MAVLink channel this message will be sent over
  114. * @param msg The MAVLink message to compress the data into
  115. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  116. * @param seq PING sequence
  117. * @param target_system 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system
  118. * @param target_component 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component.
  119. * @return length of the message in bytes (excluding serial stream start sign)
  120. */
  121. static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  122. mavlink_message_t* msg,
  123. uint64_t time_usec,uint32_t seq,uint8_t target_system,uint8_t target_component)
  124. {
  125. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  126. char buf[MAVLINK_MSG_ID_PING_LEN];
  127. _mav_put_uint64_t(buf, 0, time_usec);
  128. _mav_put_uint32_t(buf, 8, seq);
  129. _mav_put_uint8_t(buf, 12, target_system);
  130. _mav_put_uint8_t(buf, 13, target_component);
  131. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN);
  132. #else
  133. mavlink_ping_t packet;
  134. packet.time_usec = time_usec;
  135. packet.seq = seq;
  136. packet.target_system = target_system;
  137. packet.target_component = target_component;
  138. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN);
  139. #endif
  140. msg->msgid = MAVLINK_MSG_ID_PING;
  141. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  142. }
  143. /**
  144. * @brief Encode a ping struct
  145. *
  146. * @param system_id ID of this system
  147. * @param component_id ID of this component (e.g. 200 for IMU)
  148. * @param msg The MAVLink message to compress the data into
  149. * @param ping C-struct to read the message contents from
  150. */
  151. static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ping_t* ping)
  152. {
  153. return mavlink_msg_ping_pack(system_id, component_id, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  154. }
  155. /**
  156. * @brief Encode a ping struct on a channel
  157. *
  158. * @param system_id ID of this system
  159. * @param component_id ID of this component (e.g. 200 for IMU)
  160. * @param chan The MAVLink channel this message will be sent over
  161. * @param msg The MAVLink message to compress the data into
  162. * @param ping C-struct to read the message contents from
  163. */
  164. static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping)
  165. {
  166. return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  167. }
  168. /**
  169. * @brief Encode a ping struct with provided status structure
  170. *
  171. * @param system_id ID of this system
  172. * @param component_id ID of this component (e.g. 200 for IMU)
  173. * @param status MAVLink status structure
  174. * @param msg The MAVLink message to compress the data into
  175. * @param ping C-struct to read the message contents from
  176. */
  177. static inline uint16_t mavlink_msg_ping_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_ping_t* ping)
  178. {
  179. return mavlink_msg_ping_pack_status(system_id, component_id, _status, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  180. }
  181. /**
  182. * @brief Send a ping message
  183. * @param chan MAVLink channel to send the message
  184. *
  185. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  186. * @param seq PING sequence
  187. * @param target_system 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system
  188. * @param target_component 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component.
  189. */
  190. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  191. static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component)
  192. {
  193. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  194. char buf[MAVLINK_MSG_ID_PING_LEN];
  195. _mav_put_uint64_t(buf, 0, time_usec);
  196. _mav_put_uint32_t(buf, 8, seq);
  197. _mav_put_uint8_t(buf, 12, target_system);
  198. _mav_put_uint8_t(buf, 13, target_component);
  199. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  200. #else
  201. mavlink_ping_t packet;
  202. packet.time_usec = time_usec;
  203. packet.seq = seq;
  204. packet.target_system = target_system;
  205. packet.target_component = target_component;
  206. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  207. #endif
  208. }
  209. /**
  210. * @brief Send a ping message
  211. * @param chan MAVLink channel to send the message
  212. * @param struct The MAVLink struct to serialize
  213. */
  214. static inline void mavlink_msg_ping_send_struct(mavlink_channel_t chan, const mavlink_ping_t* ping)
  215. {
  216. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  217. mavlink_msg_ping_send(chan, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  218. #else
  219. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)ping, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  220. #endif
  221. }
  222. #if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  223. /*
  224. This variant of _send() can be used to save stack space by re-using
  225. memory from the receive buffer. The caller provides a
  226. mavlink_message_t which is the size of a full mavlink message. This
  227. is usually the receive buffer for the channel, and allows a reply to an
  228. incoming message with minimum stack space usage.
  229. */
  230. static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component)
  231. {
  232. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  233. char *buf = (char *)msgbuf;
  234. _mav_put_uint64_t(buf, 0, time_usec);
  235. _mav_put_uint32_t(buf, 8, seq);
  236. _mav_put_uint8_t(buf, 12, target_system);
  237. _mav_put_uint8_t(buf, 13, target_component);
  238. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  239. #else
  240. mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf;
  241. packet->time_usec = time_usec;
  242. packet->seq = seq;
  243. packet->target_system = target_system;
  244. packet->target_component = target_component;
  245. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
  246. #endif
  247. }
  248. #endif
  249. #endif
  250. // MESSAGE PING UNPACKING
  251. /**
  252. * @brief Get field time_usec from ping message
  253. *
  254. * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  255. */
  256. static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg)
  257. {
  258. return _MAV_RETURN_uint64_t(msg, 0);
  259. }
  260. /**
  261. * @brief Get field seq from ping message
  262. *
  263. * @return PING sequence
  264. */
  265. static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg)
  266. {
  267. return _MAV_RETURN_uint32_t(msg, 8);
  268. }
  269. /**
  270. * @brief Get field target_system from ping message
  271. *
  272. * @return 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system
  273. */
  274. static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg)
  275. {
  276. return _MAV_RETURN_uint8_t(msg, 12);
  277. }
  278. /**
  279. * @brief Get field target_component from ping message
  280. *
  281. * @return 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component.
  282. */
  283. static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg)
  284. {
  285. return _MAV_RETURN_uint8_t(msg, 13);
  286. }
  287. /**
  288. * @brief Decode a ping message into a struct
  289. *
  290. * @param msg The message to decode
  291. * @param ping C-struct to decode the message contents into
  292. */
  293. static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping)
  294. {
  295. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  296. ping->time_usec = mavlink_msg_ping_get_time_usec(msg);
  297. ping->seq = mavlink_msg_ping_get_seq(msg);
  298. ping->target_system = mavlink_msg_ping_get_target_system(msg);
  299. ping->target_component = mavlink_msg_ping_get_target_component(msg);
  300. #else
  301. uint8_t len = msg->len < MAVLINK_MSG_ID_PING_LEN? msg->len : MAVLINK_MSG_ID_PING_LEN;
  302. memset(ping, 0, MAVLINK_MSG_ID_PING_LEN);
  303. memcpy(ping, _MAV_PAYLOAD(msg), len);
  304. #endif
  305. }