mavlink_msg_gps_rtk.h 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513
  1. #pragma once
  2. // MESSAGE GPS_RTK PACKING
  3. #define MAVLINK_MSG_ID_GPS_RTK 127
  4. typedef struct __mavlink_gps_rtk_t {
  5. uint32_t time_last_baseline_ms; /*< [ms] Time since boot of last baseline message received.*/
  6. uint32_t tow; /*< [ms] GPS Time of Week of last baseline*/
  7. int32_t baseline_a_mm; /*< [mm] Current baseline in ECEF x or NED north component.*/
  8. int32_t baseline_b_mm; /*< [mm] Current baseline in ECEF y or NED east component.*/
  9. int32_t baseline_c_mm; /*< [mm] Current baseline in ECEF z or NED down component.*/
  10. uint32_t accuracy; /*< Current estimate of baseline accuracy.*/
  11. int32_t iar_num_hypotheses; /*< Current number of integer ambiguity hypotheses.*/
  12. uint16_t wn; /*< GPS Week Number of last baseline*/
  13. uint8_t rtk_receiver_id; /*< Identification of connected RTK receiver.*/
  14. uint8_t rtk_health; /*< GPS-specific health report for RTK data.*/
  15. uint8_t rtk_rate; /*< [Hz] Rate of baseline messages being received by GPS*/
  16. uint8_t nsats; /*< Current number of sats used for RTK calculation.*/
  17. uint8_t baseline_coords_type; /*< Coordinate system of baseline*/
  18. } mavlink_gps_rtk_t;
  19. #define MAVLINK_MSG_ID_GPS_RTK_LEN 35
  20. #define MAVLINK_MSG_ID_GPS_RTK_MIN_LEN 35
  21. #define MAVLINK_MSG_ID_127_LEN 35
  22. #define MAVLINK_MSG_ID_127_MIN_LEN 35
  23. #define MAVLINK_MSG_ID_GPS_RTK_CRC 25
  24. #define MAVLINK_MSG_ID_127_CRC 25
  25. #if MAVLINK_COMMAND_24BIT
  26. #define MAVLINK_MESSAGE_INFO_GPS_RTK { \
  27. 127, \
  28. "GPS_RTK", \
  29. 13, \
  30. { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps_rtk_t, time_last_baseline_ms) }, \
  31. { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps_rtk_t, rtk_receiver_id) }, \
  32. { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps_rtk_t, wn) }, \
  33. { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps_rtk_t, tow) }, \
  34. { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps_rtk_t, rtk_health) }, \
  35. { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps_rtk_t, rtk_rate) }, \
  36. { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps_rtk_t, nsats) }, \
  37. { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps_rtk_t, baseline_coords_type) }, \
  38. { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_rtk_t, baseline_a_mm) }, \
  39. { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_rtk_t, baseline_b_mm) }, \
  40. { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_rtk_t, baseline_c_mm) }, \
  41. { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps_rtk_t, accuracy) }, \
  42. { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps_rtk_t, iar_num_hypotheses) }, \
  43. } \
  44. }
  45. #else
  46. #define MAVLINK_MESSAGE_INFO_GPS_RTK { \
  47. "GPS_RTK", \
  48. 13, \
  49. { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps_rtk_t, time_last_baseline_ms) }, \
  50. { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps_rtk_t, rtk_receiver_id) }, \
  51. { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps_rtk_t, wn) }, \
  52. { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps_rtk_t, tow) }, \
  53. { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps_rtk_t, rtk_health) }, \
  54. { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps_rtk_t, rtk_rate) }, \
  55. { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps_rtk_t, nsats) }, \
  56. { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps_rtk_t, baseline_coords_type) }, \
  57. { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_rtk_t, baseline_a_mm) }, \
  58. { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps_rtk_t, baseline_b_mm) }, \
  59. { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps_rtk_t, baseline_c_mm) }, \
  60. { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps_rtk_t, accuracy) }, \
  61. { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps_rtk_t, iar_num_hypotheses) }, \
  62. } \
  63. }
  64. #endif
  65. /**
  66. * @brief Pack a gps_rtk message
  67. * @param system_id ID of this system
  68. * @param component_id ID of this component (e.g. 200 for IMU)
  69. * @param msg The MAVLink message to compress the data into
  70. *
  71. * @param time_last_baseline_ms [ms] Time since boot of last baseline message received.
  72. * @param rtk_receiver_id Identification of connected RTK receiver.
  73. * @param wn GPS Week Number of last baseline
  74. * @param tow [ms] GPS Time of Week of last baseline
  75. * @param rtk_health GPS-specific health report for RTK data.
  76. * @param rtk_rate [Hz] Rate of baseline messages being received by GPS
  77. * @param nsats Current number of sats used for RTK calculation.
  78. * @param baseline_coords_type Coordinate system of baseline
  79. * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component.
  80. * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component.
  81. * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component.
  82. * @param accuracy Current estimate of baseline accuracy.
  83. * @param iar_num_hypotheses Current number of integer ambiguity hypotheses.
  84. * @return length of the message in bytes (excluding serial stream start sign)
  85. */
  86. static inline uint16_t mavlink_msg_gps_rtk_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  87. uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses)
  88. {
  89. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  90. char buf[MAVLINK_MSG_ID_GPS_RTK_LEN];
  91. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  92. _mav_put_uint32_t(buf, 4, tow);
  93. _mav_put_int32_t(buf, 8, baseline_a_mm);
  94. _mav_put_int32_t(buf, 12, baseline_b_mm);
  95. _mav_put_int32_t(buf, 16, baseline_c_mm);
  96. _mav_put_uint32_t(buf, 20, accuracy);
  97. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  98. _mav_put_uint16_t(buf, 28, wn);
  99. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  100. _mav_put_uint8_t(buf, 31, rtk_health);
  101. _mav_put_uint8_t(buf, 32, rtk_rate);
  102. _mav_put_uint8_t(buf, 33, nsats);
  103. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  104. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN);
  105. #else
  106. mavlink_gps_rtk_t packet;
  107. packet.time_last_baseline_ms = time_last_baseline_ms;
  108. packet.tow = tow;
  109. packet.baseline_a_mm = baseline_a_mm;
  110. packet.baseline_b_mm = baseline_b_mm;
  111. packet.baseline_c_mm = baseline_c_mm;
  112. packet.accuracy = accuracy;
  113. packet.iar_num_hypotheses = iar_num_hypotheses;
  114. packet.wn = wn;
  115. packet.rtk_receiver_id = rtk_receiver_id;
  116. packet.rtk_health = rtk_health;
  117. packet.rtk_rate = rtk_rate;
  118. packet.nsats = nsats;
  119. packet.baseline_coords_type = baseline_coords_type;
  120. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN);
  121. #endif
  122. msg->msgid = MAVLINK_MSG_ID_GPS_RTK;
  123. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  124. }
  125. /**
  126. * @brief Pack a gps_rtk message on a channel
  127. * @param system_id ID of this system
  128. * @param component_id ID of this component (e.g. 200 for IMU)
  129. * @param chan The MAVLink channel this message will be sent over
  130. * @param msg The MAVLink message to compress the data into
  131. * @param time_last_baseline_ms [ms] Time since boot of last baseline message received.
  132. * @param rtk_receiver_id Identification of connected RTK receiver.
  133. * @param wn GPS Week Number of last baseline
  134. * @param tow [ms] GPS Time of Week of last baseline
  135. * @param rtk_health GPS-specific health report for RTK data.
  136. * @param rtk_rate [Hz] Rate of baseline messages being received by GPS
  137. * @param nsats Current number of sats used for RTK calculation.
  138. * @param baseline_coords_type Coordinate system of baseline
  139. * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component.
  140. * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component.
  141. * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component.
  142. * @param accuracy Current estimate of baseline accuracy.
  143. * @param iar_num_hypotheses Current number of integer ambiguity hypotheses.
  144. * @return length of the message in bytes (excluding serial stream start sign)
  145. */
  146. static inline uint16_t mavlink_msg_gps_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  147. mavlink_message_t* msg,
  148. uint32_t time_last_baseline_ms,uint8_t rtk_receiver_id,uint16_t wn,uint32_t tow,uint8_t rtk_health,uint8_t rtk_rate,uint8_t nsats,uint8_t baseline_coords_type,int32_t baseline_a_mm,int32_t baseline_b_mm,int32_t baseline_c_mm,uint32_t accuracy,int32_t iar_num_hypotheses)
  149. {
  150. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  151. char buf[MAVLINK_MSG_ID_GPS_RTK_LEN];
  152. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  153. _mav_put_uint32_t(buf, 4, tow);
  154. _mav_put_int32_t(buf, 8, baseline_a_mm);
  155. _mav_put_int32_t(buf, 12, baseline_b_mm);
  156. _mav_put_int32_t(buf, 16, baseline_c_mm);
  157. _mav_put_uint32_t(buf, 20, accuracy);
  158. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  159. _mav_put_uint16_t(buf, 28, wn);
  160. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  161. _mav_put_uint8_t(buf, 31, rtk_health);
  162. _mav_put_uint8_t(buf, 32, rtk_rate);
  163. _mav_put_uint8_t(buf, 33, nsats);
  164. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  165. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN);
  166. #else
  167. mavlink_gps_rtk_t packet;
  168. packet.time_last_baseline_ms = time_last_baseline_ms;
  169. packet.tow = tow;
  170. packet.baseline_a_mm = baseline_a_mm;
  171. packet.baseline_b_mm = baseline_b_mm;
  172. packet.baseline_c_mm = baseline_c_mm;
  173. packet.accuracy = accuracy;
  174. packet.iar_num_hypotheses = iar_num_hypotheses;
  175. packet.wn = wn;
  176. packet.rtk_receiver_id = rtk_receiver_id;
  177. packet.rtk_health = rtk_health;
  178. packet.rtk_rate = rtk_rate;
  179. packet.nsats = nsats;
  180. packet.baseline_coords_type = baseline_coords_type;
  181. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN);
  182. #endif
  183. msg->msgid = MAVLINK_MSG_ID_GPS_RTK;
  184. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  185. }
  186. /**
  187. * @brief Encode a gps_rtk struct
  188. *
  189. * @param system_id ID of this system
  190. * @param component_id ID of this component (e.g. 200 for IMU)
  191. * @param msg The MAVLink message to compress the data into
  192. * @param gps_rtk C-struct to read the message contents from
  193. */
  194. static inline uint16_t mavlink_msg_gps_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk)
  195. {
  196. return mavlink_msg_gps_rtk_pack(system_id, component_id, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses);
  197. }
  198. /**
  199. * @brief Encode a gps_rtk struct on a channel
  200. *
  201. * @param system_id ID of this system
  202. * @param component_id ID of this component (e.g. 200 for IMU)
  203. * @param chan The MAVLink channel this message will be sent over
  204. * @param msg The MAVLink message to compress the data into
  205. * @param gps_rtk C-struct to read the message contents from
  206. */
  207. static inline uint16_t mavlink_msg_gps_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk)
  208. {
  209. return mavlink_msg_gps_rtk_pack_chan(system_id, component_id, chan, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses);
  210. }
  211. /**
  212. * @brief Send a gps_rtk message
  213. * @param chan MAVLink channel to send the message
  214. *
  215. * @param time_last_baseline_ms [ms] Time since boot of last baseline message received.
  216. * @param rtk_receiver_id Identification of connected RTK receiver.
  217. * @param wn GPS Week Number of last baseline
  218. * @param tow [ms] GPS Time of Week of last baseline
  219. * @param rtk_health GPS-specific health report for RTK data.
  220. * @param rtk_rate [Hz] Rate of baseline messages being received by GPS
  221. * @param nsats Current number of sats used for RTK calculation.
  222. * @param baseline_coords_type Coordinate system of baseline
  223. * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component.
  224. * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component.
  225. * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component.
  226. * @param accuracy Current estimate of baseline accuracy.
  227. * @param iar_num_hypotheses Current number of integer ambiguity hypotheses.
  228. */
  229. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  230. static inline void mavlink_msg_gps_rtk_send(mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses)
  231. {
  232. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  233. char buf[MAVLINK_MSG_ID_GPS_RTK_LEN];
  234. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  235. _mav_put_uint32_t(buf, 4, tow);
  236. _mav_put_int32_t(buf, 8, baseline_a_mm);
  237. _mav_put_int32_t(buf, 12, baseline_b_mm);
  238. _mav_put_int32_t(buf, 16, baseline_c_mm);
  239. _mav_put_uint32_t(buf, 20, accuracy);
  240. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  241. _mav_put_uint16_t(buf, 28, wn);
  242. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  243. _mav_put_uint8_t(buf, 31, rtk_health);
  244. _mav_put_uint8_t(buf, 32, rtk_rate);
  245. _mav_put_uint8_t(buf, 33, nsats);
  246. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  247. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  248. #else
  249. mavlink_gps_rtk_t packet;
  250. packet.time_last_baseline_ms = time_last_baseline_ms;
  251. packet.tow = tow;
  252. packet.baseline_a_mm = baseline_a_mm;
  253. packet.baseline_b_mm = baseline_b_mm;
  254. packet.baseline_c_mm = baseline_c_mm;
  255. packet.accuracy = accuracy;
  256. packet.iar_num_hypotheses = iar_num_hypotheses;
  257. packet.wn = wn;
  258. packet.rtk_receiver_id = rtk_receiver_id;
  259. packet.rtk_health = rtk_health;
  260. packet.rtk_rate = rtk_rate;
  261. packet.nsats = nsats;
  262. packet.baseline_coords_type = baseline_coords_type;
  263. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  264. #endif
  265. }
  266. /**
  267. * @brief Send a gps_rtk message
  268. * @param chan MAVLink channel to send the message
  269. * @param struct The MAVLink struct to serialize
  270. */
  271. static inline void mavlink_msg_gps_rtk_send_struct(mavlink_channel_t chan, const mavlink_gps_rtk_t* gps_rtk)
  272. {
  273. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  274. mavlink_msg_gps_rtk_send(chan, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses);
  275. #else
  276. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)gps_rtk, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  277. #endif
  278. }
  279. #if MAVLINK_MSG_ID_GPS_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  280. /*
  281. This variant of _send() can be used to save stack space by re-using
  282. memory from the receive buffer. The caller provides a
  283. mavlink_message_t which is the size of a full mavlink message. This
  284. is usually the receive buffer for the channel, and allows a reply to an
  285. incoming message with minimum stack space usage.
  286. */
  287. static inline void mavlink_msg_gps_rtk_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses)
  288. {
  289. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  290. char *buf = (char *)msgbuf;
  291. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  292. _mav_put_uint32_t(buf, 4, tow);
  293. _mav_put_int32_t(buf, 8, baseline_a_mm);
  294. _mav_put_int32_t(buf, 12, baseline_b_mm);
  295. _mav_put_int32_t(buf, 16, baseline_c_mm);
  296. _mav_put_uint32_t(buf, 20, accuracy);
  297. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  298. _mav_put_uint16_t(buf, 28, wn);
  299. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  300. _mav_put_uint8_t(buf, 31, rtk_health);
  301. _mav_put_uint8_t(buf, 32, rtk_rate);
  302. _mav_put_uint8_t(buf, 33, nsats);
  303. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  304. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, buf, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  305. #else
  306. mavlink_gps_rtk_t *packet = (mavlink_gps_rtk_t *)msgbuf;
  307. packet->time_last_baseline_ms = time_last_baseline_ms;
  308. packet->tow = tow;
  309. packet->baseline_a_mm = baseline_a_mm;
  310. packet->baseline_b_mm = baseline_b_mm;
  311. packet->baseline_c_mm = baseline_c_mm;
  312. packet->accuracy = accuracy;
  313. packet->iar_num_hypotheses = iar_num_hypotheses;
  314. packet->wn = wn;
  315. packet->rtk_receiver_id = rtk_receiver_id;
  316. packet->rtk_health = rtk_health;
  317. packet->rtk_rate = rtk_rate;
  318. packet->nsats = nsats;
  319. packet->baseline_coords_type = baseline_coords_type;
  320. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC);
  321. #endif
  322. }
  323. #endif
  324. #endif
  325. // MESSAGE GPS_RTK UNPACKING
  326. /**
  327. * @brief Get field time_last_baseline_ms from gps_rtk message
  328. *
  329. * @return [ms] Time since boot of last baseline message received.
  330. */
  331. static inline uint32_t mavlink_msg_gps_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg)
  332. {
  333. return _MAV_RETURN_uint32_t(msg, 0);
  334. }
  335. /**
  336. * @brief Get field rtk_receiver_id from gps_rtk message
  337. *
  338. * @return Identification of connected RTK receiver.
  339. */
  340. static inline uint8_t mavlink_msg_gps_rtk_get_rtk_receiver_id(const mavlink_message_t* msg)
  341. {
  342. return _MAV_RETURN_uint8_t(msg, 30);
  343. }
  344. /**
  345. * @brief Get field wn from gps_rtk message
  346. *
  347. * @return GPS Week Number of last baseline
  348. */
  349. static inline uint16_t mavlink_msg_gps_rtk_get_wn(const mavlink_message_t* msg)
  350. {
  351. return _MAV_RETURN_uint16_t(msg, 28);
  352. }
  353. /**
  354. * @brief Get field tow from gps_rtk message
  355. *
  356. * @return [ms] GPS Time of Week of last baseline
  357. */
  358. static inline uint32_t mavlink_msg_gps_rtk_get_tow(const mavlink_message_t* msg)
  359. {
  360. return _MAV_RETURN_uint32_t(msg, 4);
  361. }
  362. /**
  363. * @brief Get field rtk_health from gps_rtk message
  364. *
  365. * @return GPS-specific health report for RTK data.
  366. */
  367. static inline uint8_t mavlink_msg_gps_rtk_get_rtk_health(const mavlink_message_t* msg)
  368. {
  369. return _MAV_RETURN_uint8_t(msg, 31);
  370. }
  371. /**
  372. * @brief Get field rtk_rate from gps_rtk message
  373. *
  374. * @return [Hz] Rate of baseline messages being received by GPS
  375. */
  376. static inline uint8_t mavlink_msg_gps_rtk_get_rtk_rate(const mavlink_message_t* msg)
  377. {
  378. return _MAV_RETURN_uint8_t(msg, 32);
  379. }
  380. /**
  381. * @brief Get field nsats from gps_rtk message
  382. *
  383. * @return Current number of sats used for RTK calculation.
  384. */
  385. static inline uint8_t mavlink_msg_gps_rtk_get_nsats(const mavlink_message_t* msg)
  386. {
  387. return _MAV_RETURN_uint8_t(msg, 33);
  388. }
  389. /**
  390. * @brief Get field baseline_coords_type from gps_rtk message
  391. *
  392. * @return Coordinate system of baseline
  393. */
  394. static inline uint8_t mavlink_msg_gps_rtk_get_baseline_coords_type(const mavlink_message_t* msg)
  395. {
  396. return _MAV_RETURN_uint8_t(msg, 34);
  397. }
  398. /**
  399. * @brief Get field baseline_a_mm from gps_rtk message
  400. *
  401. * @return [mm] Current baseline in ECEF x or NED north component.
  402. */
  403. static inline int32_t mavlink_msg_gps_rtk_get_baseline_a_mm(const mavlink_message_t* msg)
  404. {
  405. return _MAV_RETURN_int32_t(msg, 8);
  406. }
  407. /**
  408. * @brief Get field baseline_b_mm from gps_rtk message
  409. *
  410. * @return [mm] Current baseline in ECEF y or NED east component.
  411. */
  412. static inline int32_t mavlink_msg_gps_rtk_get_baseline_b_mm(const mavlink_message_t* msg)
  413. {
  414. return _MAV_RETURN_int32_t(msg, 12);
  415. }
  416. /**
  417. * @brief Get field baseline_c_mm from gps_rtk message
  418. *
  419. * @return [mm] Current baseline in ECEF z or NED down component.
  420. */
  421. static inline int32_t mavlink_msg_gps_rtk_get_baseline_c_mm(const mavlink_message_t* msg)
  422. {
  423. return _MAV_RETURN_int32_t(msg, 16);
  424. }
  425. /**
  426. * @brief Get field accuracy from gps_rtk message
  427. *
  428. * @return Current estimate of baseline accuracy.
  429. */
  430. static inline uint32_t mavlink_msg_gps_rtk_get_accuracy(const mavlink_message_t* msg)
  431. {
  432. return _MAV_RETURN_uint32_t(msg, 20);
  433. }
  434. /**
  435. * @brief Get field iar_num_hypotheses from gps_rtk message
  436. *
  437. * @return Current number of integer ambiguity hypotheses.
  438. */
  439. static inline int32_t mavlink_msg_gps_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg)
  440. {
  441. return _MAV_RETURN_int32_t(msg, 24);
  442. }
  443. /**
  444. * @brief Decode a gps_rtk message into a struct
  445. *
  446. * @param msg The message to decode
  447. * @param gps_rtk C-struct to decode the message contents into
  448. */
  449. static inline void mavlink_msg_gps_rtk_decode(const mavlink_message_t* msg, mavlink_gps_rtk_t* gps_rtk)
  450. {
  451. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  452. gps_rtk->time_last_baseline_ms = mavlink_msg_gps_rtk_get_time_last_baseline_ms(msg);
  453. gps_rtk->tow = mavlink_msg_gps_rtk_get_tow(msg);
  454. gps_rtk->baseline_a_mm = mavlink_msg_gps_rtk_get_baseline_a_mm(msg);
  455. gps_rtk->baseline_b_mm = mavlink_msg_gps_rtk_get_baseline_b_mm(msg);
  456. gps_rtk->baseline_c_mm = mavlink_msg_gps_rtk_get_baseline_c_mm(msg);
  457. gps_rtk->accuracy = mavlink_msg_gps_rtk_get_accuracy(msg);
  458. gps_rtk->iar_num_hypotheses = mavlink_msg_gps_rtk_get_iar_num_hypotheses(msg);
  459. gps_rtk->wn = mavlink_msg_gps_rtk_get_wn(msg);
  460. gps_rtk->rtk_receiver_id = mavlink_msg_gps_rtk_get_rtk_receiver_id(msg);
  461. gps_rtk->rtk_health = mavlink_msg_gps_rtk_get_rtk_health(msg);
  462. gps_rtk->rtk_rate = mavlink_msg_gps_rtk_get_rtk_rate(msg);
  463. gps_rtk->nsats = mavlink_msg_gps_rtk_get_nsats(msg);
  464. gps_rtk->baseline_coords_type = mavlink_msg_gps_rtk_get_baseline_coords_type(msg);
  465. #else
  466. uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_RTK_LEN? msg->len : MAVLINK_MSG_ID_GPS_RTK_LEN;
  467. memset(gps_rtk, 0, MAVLINK_MSG_ID_GPS_RTK_LEN);
  468. memcpy(gps_rtk, _MAV_PAYLOAD(msg), len);
  469. #endif
  470. }