mavlink_msg_ping.h 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288
  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 on a channel
  73. * @param system_id ID of this system
  74. * @param component_id ID of this component (e.g. 200 for IMU)
  75. * @param chan The MAVLink channel this message will be sent over
  76. * @param msg The MAVLink message to compress the data into
  77. * @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.
  78. * @param seq PING sequence
  79. * @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
  80. * @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.
  81. * @return length of the message in bytes (excluding serial stream start sign)
  82. */
  83. static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  84. 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. 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);
  104. }
  105. /**
  106. * @brief Encode a ping struct
  107. *
  108. * @param system_id ID of this system
  109. * @param component_id ID of this component (e.g. 200 for IMU)
  110. * @param msg The MAVLink message to compress the data into
  111. * @param ping C-struct to read the message contents from
  112. */
  113. 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)
  114. {
  115. return mavlink_msg_ping_pack(system_id, component_id, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  116. }
  117. /**
  118. * @brief Encode a ping struct on a channel
  119. *
  120. * @param system_id ID of this system
  121. * @param component_id ID of this component (e.g. 200 for IMU)
  122. * @param chan The MAVLink channel this message will be sent over
  123. * @param msg The MAVLink message to compress the data into
  124. * @param ping C-struct to read the message contents from
  125. */
  126. 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)
  127. {
  128. return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  129. }
  130. /**
  131. * @brief Send a ping message
  132. * @param chan MAVLink channel to send the message
  133. *
  134. * @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.
  135. * @param seq PING sequence
  136. * @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
  137. * @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.
  138. */
  139. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  140. 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)
  141. {
  142. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  143. char buf[MAVLINK_MSG_ID_PING_LEN];
  144. _mav_put_uint64_t(buf, 0, time_usec);
  145. _mav_put_uint32_t(buf, 8, seq);
  146. _mav_put_uint8_t(buf, 12, target_system);
  147. _mav_put_uint8_t(buf, 13, target_component);
  148. _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);
  149. #else
  150. mavlink_ping_t packet;
  151. packet.time_usec = time_usec;
  152. packet.seq = seq;
  153. packet.target_system = target_system;
  154. packet.target_component = target_component;
  155. _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);
  156. #endif
  157. }
  158. /**
  159. * @brief Send a ping message
  160. * @param chan MAVLink channel to send the message
  161. * @param struct The MAVLink struct to serialize
  162. */
  163. static inline void mavlink_msg_ping_send_struct(mavlink_channel_t chan, const mavlink_ping_t* ping)
  164. {
  165. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  166. mavlink_msg_ping_send(chan, ping->time_usec, ping->seq, ping->target_system, ping->target_component);
  167. #else
  168. _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);
  169. #endif
  170. }
  171. #if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  172. /*
  173. This variant of _send() can be used to save stack space by re-using
  174. memory from the receive buffer. The caller provides a
  175. mavlink_message_t which is the size of a full mavlink message. This
  176. is usually the receive buffer for the channel, and allows a reply to an
  177. incoming message with minimum stack space usage.
  178. */
  179. 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)
  180. {
  181. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  182. char *buf = (char *)msgbuf;
  183. _mav_put_uint64_t(buf, 0, time_usec);
  184. _mav_put_uint32_t(buf, 8, seq);
  185. _mav_put_uint8_t(buf, 12, target_system);
  186. _mav_put_uint8_t(buf, 13, target_component);
  187. _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);
  188. #else
  189. mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf;
  190. packet->time_usec = time_usec;
  191. packet->seq = seq;
  192. packet->target_system = target_system;
  193. packet->target_component = target_component;
  194. _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);
  195. #endif
  196. }
  197. #endif
  198. #endif
  199. // MESSAGE PING UNPACKING
  200. /**
  201. * @brief Get field time_usec from ping message
  202. *
  203. * @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.
  204. */
  205. static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg)
  206. {
  207. return _MAV_RETURN_uint64_t(msg, 0);
  208. }
  209. /**
  210. * @brief Get field seq from ping message
  211. *
  212. * @return PING sequence
  213. */
  214. static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg)
  215. {
  216. return _MAV_RETURN_uint32_t(msg, 8);
  217. }
  218. /**
  219. * @brief Get field target_system from ping message
  220. *
  221. * @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
  222. */
  223. static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg)
  224. {
  225. return _MAV_RETURN_uint8_t(msg, 12);
  226. }
  227. /**
  228. * @brief Get field target_component from ping message
  229. *
  230. * @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.
  231. */
  232. static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg)
  233. {
  234. return _MAV_RETURN_uint8_t(msg, 13);
  235. }
  236. /**
  237. * @brief Decode a ping message into a struct
  238. *
  239. * @param msg The message to decode
  240. * @param ping C-struct to decode the message contents into
  241. */
  242. static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping)
  243. {
  244. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  245. ping->time_usec = mavlink_msg_ping_get_time_usec(msg);
  246. ping->seq = mavlink_msg_ping_get_seq(msg);
  247. ping->target_system = mavlink_msg_ping_get_target_system(msg);
  248. ping->target_component = mavlink_msg_ping_get_target_component(msg);
  249. #else
  250. uint8_t len = msg->len < MAVLINK_MSG_ID_PING_LEN? msg->len : MAVLINK_MSG_ID_PING_LEN;
  251. memset(ping, 0, MAVLINK_MSG_ID_PING_LEN);
  252. memcpy(ping, _MAV_PAYLOAD(msg), len);
  253. #endif
  254. }