mavlink_msg_gps2_rtk.h 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596
  1. #pragma once
  2. // MESSAGE GPS2_RTK PACKING
  3. #define MAVLINK_MSG_ID_GPS2_RTK 128
  4. typedef struct __mavlink_gps2_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_gps2_rtk_t;
  19. #define MAVLINK_MSG_ID_GPS2_RTK_LEN 35
  20. #define MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN 35
  21. #define MAVLINK_MSG_ID_128_LEN 35
  22. #define MAVLINK_MSG_ID_128_MIN_LEN 35
  23. #define MAVLINK_MSG_ID_GPS2_RTK_CRC 226
  24. #define MAVLINK_MSG_ID_128_CRC 226
  25. #if MAVLINK_COMMAND_24BIT
  26. #define MAVLINK_MESSAGE_INFO_GPS2_RTK { \
  27. 128, \
  28. "GPS2_RTK", \
  29. 13, \
  30. { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps2_rtk_t, time_last_baseline_ms) }, \
  31. { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps2_rtk_t, rtk_receiver_id) }, \
  32. { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_rtk_t, wn) }, \
  33. { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps2_rtk_t, tow) }, \
  34. { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps2_rtk_t, rtk_health) }, \
  35. { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_rtk_t, rtk_rate) }, \
  36. { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_rtk_t, nsats) }, \
  37. { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_rtk_t, baseline_coords_type) }, \
  38. { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_rtk_t, baseline_a_mm) }, \
  39. { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_rtk_t, baseline_b_mm) }, \
  40. { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_rtk_t, baseline_c_mm) }, \
  41. { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_rtk_t, accuracy) }, \
  42. { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps2_rtk_t, iar_num_hypotheses) }, \
  43. } \
  44. }
  45. #else
  46. #define MAVLINK_MESSAGE_INFO_GPS2_RTK { \
  47. "GPS2_RTK", \
  48. 13, \
  49. { { "time_last_baseline_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gps2_rtk_t, time_last_baseline_ms) }, \
  50. { "rtk_receiver_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gps2_rtk_t, rtk_receiver_id) }, \
  51. { "wn", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_rtk_t, wn) }, \
  52. { "tow", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gps2_rtk_t, tow) }, \
  53. { "rtk_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gps2_rtk_t, rtk_health) }, \
  54. { "rtk_rate", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_rtk_t, rtk_rate) }, \
  55. { "nsats", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_rtk_t, nsats) }, \
  56. { "baseline_coords_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_rtk_t, baseline_coords_type) }, \
  57. { "baseline_a_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_rtk_t, baseline_a_mm) }, \
  58. { "baseline_b_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_rtk_t, baseline_b_mm) }, \
  59. { "baseline_c_mm", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_rtk_t, baseline_c_mm) }, \
  60. { "accuracy", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_rtk_t, accuracy) }, \
  61. { "iar_num_hypotheses", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_gps2_rtk_t, iar_num_hypotheses) }, \
  62. } \
  63. }
  64. #endif
  65. /**
  66. * @brief Pack a gps2_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_gps2_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_GPS2_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_GPS2_RTK_LEN);
  105. #else
  106. mavlink_gps2_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_GPS2_RTK_LEN);
  121. #endif
  122. msg->msgid = MAVLINK_MSG_ID_GPS2_RTK;
  123. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  124. }
  125. /**
  126. * @brief Pack a gps2_rtk message
  127. * @param system_id ID of this system
  128. * @param component_id ID of this component (e.g. 200 for IMU)
  129. * @param status MAVLink status structure
  130. * @param msg The MAVLink message to compress the data into
  131. *
  132. * @param time_last_baseline_ms [ms] Time since boot of last baseline message received.
  133. * @param rtk_receiver_id Identification of connected RTK receiver.
  134. * @param wn GPS Week Number of last baseline
  135. * @param tow [ms] GPS Time of Week of last baseline
  136. * @param rtk_health GPS-specific health report for RTK data.
  137. * @param rtk_rate [Hz] Rate of baseline messages being received by GPS
  138. * @param nsats Current number of sats used for RTK calculation.
  139. * @param baseline_coords_type Coordinate system of baseline
  140. * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component.
  141. * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component.
  142. * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component.
  143. * @param accuracy Current estimate of baseline accuracy.
  144. * @param iar_num_hypotheses Current number of integer ambiguity hypotheses.
  145. * @return length of the message in bytes (excluding serial stream start sign)
  146. */
  147. static inline uint16_t mavlink_msg_gps2_rtk_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, 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_GPS2_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_GPS2_RTK_LEN);
  166. #else
  167. mavlink_gps2_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_GPS2_RTK_LEN);
  182. #endif
  183. msg->msgid = MAVLINK_MSG_ID_GPS2_RTK;
  184. #if MAVLINK_CRC_EXTRA
  185. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  186. #else
  187. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN);
  188. #endif
  189. }
  190. /**
  191. * @brief Pack a gps2_rtk message on a channel
  192. * @param system_id ID of this system
  193. * @param component_id ID of this component (e.g. 200 for IMU)
  194. * @param chan The MAVLink channel this message will be sent over
  195. * @param msg The MAVLink message to compress the data into
  196. * @param time_last_baseline_ms [ms] Time since boot of last baseline message received.
  197. * @param rtk_receiver_id Identification of connected RTK receiver.
  198. * @param wn GPS Week Number of last baseline
  199. * @param tow [ms] GPS Time of Week of last baseline
  200. * @param rtk_health GPS-specific health report for RTK data.
  201. * @param rtk_rate [Hz] Rate of baseline messages being received by GPS
  202. * @param nsats Current number of sats used for RTK calculation.
  203. * @param baseline_coords_type Coordinate system of baseline
  204. * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component.
  205. * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component.
  206. * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component.
  207. * @param accuracy Current estimate of baseline accuracy.
  208. * @param iar_num_hypotheses Current number of integer ambiguity hypotheses.
  209. * @return length of the message in bytes (excluding serial stream start sign)
  210. */
  211. static inline uint16_t mavlink_msg_gps2_rtk_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  212. mavlink_message_t* msg,
  213. 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)
  214. {
  215. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  216. char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN];
  217. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  218. _mav_put_uint32_t(buf, 4, tow);
  219. _mav_put_int32_t(buf, 8, baseline_a_mm);
  220. _mav_put_int32_t(buf, 12, baseline_b_mm);
  221. _mav_put_int32_t(buf, 16, baseline_c_mm);
  222. _mav_put_uint32_t(buf, 20, accuracy);
  223. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  224. _mav_put_uint16_t(buf, 28, wn);
  225. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  226. _mav_put_uint8_t(buf, 31, rtk_health);
  227. _mav_put_uint8_t(buf, 32, rtk_rate);
  228. _mav_put_uint8_t(buf, 33, nsats);
  229. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  230. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN);
  231. #else
  232. mavlink_gps2_rtk_t packet;
  233. packet.time_last_baseline_ms = time_last_baseline_ms;
  234. packet.tow = tow;
  235. packet.baseline_a_mm = baseline_a_mm;
  236. packet.baseline_b_mm = baseline_b_mm;
  237. packet.baseline_c_mm = baseline_c_mm;
  238. packet.accuracy = accuracy;
  239. packet.iar_num_hypotheses = iar_num_hypotheses;
  240. packet.wn = wn;
  241. packet.rtk_receiver_id = rtk_receiver_id;
  242. packet.rtk_health = rtk_health;
  243. packet.rtk_rate = rtk_rate;
  244. packet.nsats = nsats;
  245. packet.baseline_coords_type = baseline_coords_type;
  246. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN);
  247. #endif
  248. msg->msgid = MAVLINK_MSG_ID_GPS2_RTK;
  249. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  250. }
  251. /**
  252. * @brief Encode a gps2_rtk struct
  253. *
  254. * @param system_id ID of this system
  255. * @param component_id ID of this component (e.g. 200 for IMU)
  256. * @param msg The MAVLink message to compress the data into
  257. * @param gps2_rtk C-struct to read the message contents from
  258. */
  259. static inline uint16_t mavlink_msg_gps2_rtk_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk)
  260. {
  261. return mavlink_msg_gps2_rtk_pack(system_id, component_id, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses);
  262. }
  263. /**
  264. * @brief Encode a gps2_rtk struct on a channel
  265. *
  266. * @param system_id ID of this system
  267. * @param component_id ID of this component (e.g. 200 for IMU)
  268. * @param chan The MAVLink channel this message will be sent over
  269. * @param msg The MAVLink message to compress the data into
  270. * @param gps2_rtk C-struct to read the message contents from
  271. */
  272. static inline uint16_t mavlink_msg_gps2_rtk_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk)
  273. {
  274. return mavlink_msg_gps2_rtk_pack_chan(system_id, component_id, chan, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses);
  275. }
  276. /**
  277. * @brief Encode a gps2_rtk struct with provided status structure
  278. *
  279. * @param system_id ID of this system
  280. * @param component_id ID of this component (e.g. 200 for IMU)
  281. * @param status MAVLink status structure
  282. * @param msg The MAVLink message to compress the data into
  283. * @param gps2_rtk C-struct to read the message contents from
  284. */
  285. static inline uint16_t mavlink_msg_gps2_rtk_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk)
  286. {
  287. return mavlink_msg_gps2_rtk_pack_status(system_id, component_id, _status, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses);
  288. }
  289. /**
  290. * @brief Send a gps2_rtk message
  291. * @param chan MAVLink channel to send the message
  292. *
  293. * @param time_last_baseline_ms [ms] Time since boot of last baseline message received.
  294. * @param rtk_receiver_id Identification of connected RTK receiver.
  295. * @param wn GPS Week Number of last baseline
  296. * @param tow [ms] GPS Time of Week of last baseline
  297. * @param rtk_health GPS-specific health report for RTK data.
  298. * @param rtk_rate [Hz] Rate of baseline messages being received by GPS
  299. * @param nsats Current number of sats used for RTK calculation.
  300. * @param baseline_coords_type Coordinate system of baseline
  301. * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component.
  302. * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component.
  303. * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component.
  304. * @param accuracy Current estimate of baseline accuracy.
  305. * @param iar_num_hypotheses Current number of integer ambiguity hypotheses.
  306. */
  307. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  308. static inline void mavlink_msg_gps2_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)
  309. {
  310. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  311. char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN];
  312. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  313. _mav_put_uint32_t(buf, 4, tow);
  314. _mav_put_int32_t(buf, 8, baseline_a_mm);
  315. _mav_put_int32_t(buf, 12, baseline_b_mm);
  316. _mav_put_int32_t(buf, 16, baseline_c_mm);
  317. _mav_put_uint32_t(buf, 20, accuracy);
  318. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  319. _mav_put_uint16_t(buf, 28, wn);
  320. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  321. _mav_put_uint8_t(buf, 31, rtk_health);
  322. _mav_put_uint8_t(buf, 32, rtk_rate);
  323. _mav_put_uint8_t(buf, 33, nsats);
  324. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  325. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  326. #else
  327. mavlink_gps2_rtk_t packet;
  328. packet.time_last_baseline_ms = time_last_baseline_ms;
  329. packet.tow = tow;
  330. packet.baseline_a_mm = baseline_a_mm;
  331. packet.baseline_b_mm = baseline_b_mm;
  332. packet.baseline_c_mm = baseline_c_mm;
  333. packet.accuracy = accuracy;
  334. packet.iar_num_hypotheses = iar_num_hypotheses;
  335. packet.wn = wn;
  336. packet.rtk_receiver_id = rtk_receiver_id;
  337. packet.rtk_health = rtk_health;
  338. packet.rtk_rate = rtk_rate;
  339. packet.nsats = nsats;
  340. packet.baseline_coords_type = baseline_coords_type;
  341. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  342. #endif
  343. }
  344. /**
  345. * @brief Send a gps2_rtk message
  346. * @param chan MAVLink channel to send the message
  347. * @param struct The MAVLink struct to serialize
  348. */
  349. static inline void mavlink_msg_gps2_rtk_send_struct(mavlink_channel_t chan, const mavlink_gps2_rtk_t* gps2_rtk)
  350. {
  351. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  352. mavlink_msg_gps2_rtk_send(chan, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses);
  353. #else
  354. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)gps2_rtk, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  355. #endif
  356. }
  357. #if MAVLINK_MSG_ID_GPS2_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  358. /*
  359. This variant of _send() can be used to save stack space by re-using
  360. memory from the receive buffer. The caller provides a
  361. mavlink_message_t which is the size of a full mavlink message. This
  362. is usually the receive buffer for the channel, and allows a reply to an
  363. incoming message with minimum stack space usage.
  364. */
  365. static inline void mavlink_msg_gps2_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)
  366. {
  367. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  368. char *buf = (char *)msgbuf;
  369. _mav_put_uint32_t(buf, 0, time_last_baseline_ms);
  370. _mav_put_uint32_t(buf, 4, tow);
  371. _mav_put_int32_t(buf, 8, baseline_a_mm);
  372. _mav_put_int32_t(buf, 12, baseline_b_mm);
  373. _mav_put_int32_t(buf, 16, baseline_c_mm);
  374. _mav_put_uint32_t(buf, 20, accuracy);
  375. _mav_put_int32_t(buf, 24, iar_num_hypotheses);
  376. _mav_put_uint16_t(buf, 28, wn);
  377. _mav_put_uint8_t(buf, 30, rtk_receiver_id);
  378. _mav_put_uint8_t(buf, 31, rtk_health);
  379. _mav_put_uint8_t(buf, 32, rtk_rate);
  380. _mav_put_uint8_t(buf, 33, nsats);
  381. _mav_put_uint8_t(buf, 34, baseline_coords_type);
  382. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, buf, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  383. #else
  384. mavlink_gps2_rtk_t *packet = (mavlink_gps2_rtk_t *)msgbuf;
  385. packet->time_last_baseline_ms = time_last_baseline_ms;
  386. packet->tow = tow;
  387. packet->baseline_a_mm = baseline_a_mm;
  388. packet->baseline_b_mm = baseline_b_mm;
  389. packet->baseline_c_mm = baseline_c_mm;
  390. packet->accuracy = accuracy;
  391. packet->iar_num_hypotheses = iar_num_hypotheses;
  392. packet->wn = wn;
  393. packet->rtk_receiver_id = rtk_receiver_id;
  394. packet->rtk_health = rtk_health;
  395. packet->rtk_rate = rtk_rate;
  396. packet->nsats = nsats;
  397. packet->baseline_coords_type = baseline_coords_type;
  398. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RTK, (const char *)packet, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC);
  399. #endif
  400. }
  401. #endif
  402. #endif
  403. // MESSAGE GPS2_RTK UNPACKING
  404. /**
  405. * @brief Get field time_last_baseline_ms from gps2_rtk message
  406. *
  407. * @return [ms] Time since boot of last baseline message received.
  408. */
  409. static inline uint32_t mavlink_msg_gps2_rtk_get_time_last_baseline_ms(const mavlink_message_t* msg)
  410. {
  411. return _MAV_RETURN_uint32_t(msg, 0);
  412. }
  413. /**
  414. * @brief Get field rtk_receiver_id from gps2_rtk message
  415. *
  416. * @return Identification of connected RTK receiver.
  417. */
  418. static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_receiver_id(const mavlink_message_t* msg)
  419. {
  420. return _MAV_RETURN_uint8_t(msg, 30);
  421. }
  422. /**
  423. * @brief Get field wn from gps2_rtk message
  424. *
  425. * @return GPS Week Number of last baseline
  426. */
  427. static inline uint16_t mavlink_msg_gps2_rtk_get_wn(const mavlink_message_t* msg)
  428. {
  429. return _MAV_RETURN_uint16_t(msg, 28);
  430. }
  431. /**
  432. * @brief Get field tow from gps2_rtk message
  433. *
  434. * @return [ms] GPS Time of Week of last baseline
  435. */
  436. static inline uint32_t mavlink_msg_gps2_rtk_get_tow(const mavlink_message_t* msg)
  437. {
  438. return _MAV_RETURN_uint32_t(msg, 4);
  439. }
  440. /**
  441. * @brief Get field rtk_health from gps2_rtk message
  442. *
  443. * @return GPS-specific health report for RTK data.
  444. */
  445. static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_health(const mavlink_message_t* msg)
  446. {
  447. return _MAV_RETURN_uint8_t(msg, 31);
  448. }
  449. /**
  450. * @brief Get field rtk_rate from gps2_rtk message
  451. *
  452. * @return [Hz] Rate of baseline messages being received by GPS
  453. */
  454. static inline uint8_t mavlink_msg_gps2_rtk_get_rtk_rate(const mavlink_message_t* msg)
  455. {
  456. return _MAV_RETURN_uint8_t(msg, 32);
  457. }
  458. /**
  459. * @brief Get field nsats from gps2_rtk message
  460. *
  461. * @return Current number of sats used for RTK calculation.
  462. */
  463. static inline uint8_t mavlink_msg_gps2_rtk_get_nsats(const mavlink_message_t* msg)
  464. {
  465. return _MAV_RETURN_uint8_t(msg, 33);
  466. }
  467. /**
  468. * @brief Get field baseline_coords_type from gps2_rtk message
  469. *
  470. * @return Coordinate system of baseline
  471. */
  472. static inline uint8_t mavlink_msg_gps2_rtk_get_baseline_coords_type(const mavlink_message_t* msg)
  473. {
  474. return _MAV_RETURN_uint8_t(msg, 34);
  475. }
  476. /**
  477. * @brief Get field baseline_a_mm from gps2_rtk message
  478. *
  479. * @return [mm] Current baseline in ECEF x or NED north component.
  480. */
  481. static inline int32_t mavlink_msg_gps2_rtk_get_baseline_a_mm(const mavlink_message_t* msg)
  482. {
  483. return _MAV_RETURN_int32_t(msg, 8);
  484. }
  485. /**
  486. * @brief Get field baseline_b_mm from gps2_rtk message
  487. *
  488. * @return [mm] Current baseline in ECEF y or NED east component.
  489. */
  490. static inline int32_t mavlink_msg_gps2_rtk_get_baseline_b_mm(const mavlink_message_t* msg)
  491. {
  492. return _MAV_RETURN_int32_t(msg, 12);
  493. }
  494. /**
  495. * @brief Get field baseline_c_mm from gps2_rtk message
  496. *
  497. * @return [mm] Current baseline in ECEF z or NED down component.
  498. */
  499. static inline int32_t mavlink_msg_gps2_rtk_get_baseline_c_mm(const mavlink_message_t* msg)
  500. {
  501. return _MAV_RETURN_int32_t(msg, 16);
  502. }
  503. /**
  504. * @brief Get field accuracy from gps2_rtk message
  505. *
  506. * @return Current estimate of baseline accuracy.
  507. */
  508. static inline uint32_t mavlink_msg_gps2_rtk_get_accuracy(const mavlink_message_t* msg)
  509. {
  510. return _MAV_RETURN_uint32_t(msg, 20);
  511. }
  512. /**
  513. * @brief Get field iar_num_hypotheses from gps2_rtk message
  514. *
  515. * @return Current number of integer ambiguity hypotheses.
  516. */
  517. static inline int32_t mavlink_msg_gps2_rtk_get_iar_num_hypotheses(const mavlink_message_t* msg)
  518. {
  519. return _MAV_RETURN_int32_t(msg, 24);
  520. }
  521. /**
  522. * @brief Decode a gps2_rtk message into a struct
  523. *
  524. * @param msg The message to decode
  525. * @param gps2_rtk C-struct to decode the message contents into
  526. */
  527. static inline void mavlink_msg_gps2_rtk_decode(const mavlink_message_t* msg, mavlink_gps2_rtk_t* gps2_rtk)
  528. {
  529. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  530. gps2_rtk->time_last_baseline_ms = mavlink_msg_gps2_rtk_get_time_last_baseline_ms(msg);
  531. gps2_rtk->tow = mavlink_msg_gps2_rtk_get_tow(msg);
  532. gps2_rtk->baseline_a_mm = mavlink_msg_gps2_rtk_get_baseline_a_mm(msg);
  533. gps2_rtk->baseline_b_mm = mavlink_msg_gps2_rtk_get_baseline_b_mm(msg);
  534. gps2_rtk->baseline_c_mm = mavlink_msg_gps2_rtk_get_baseline_c_mm(msg);
  535. gps2_rtk->accuracy = mavlink_msg_gps2_rtk_get_accuracy(msg);
  536. gps2_rtk->iar_num_hypotheses = mavlink_msg_gps2_rtk_get_iar_num_hypotheses(msg);
  537. gps2_rtk->wn = mavlink_msg_gps2_rtk_get_wn(msg);
  538. gps2_rtk->rtk_receiver_id = mavlink_msg_gps2_rtk_get_rtk_receiver_id(msg);
  539. gps2_rtk->rtk_health = mavlink_msg_gps2_rtk_get_rtk_health(msg);
  540. gps2_rtk->rtk_rate = mavlink_msg_gps2_rtk_get_rtk_rate(msg);
  541. gps2_rtk->nsats = mavlink_msg_gps2_rtk_get_nsats(msg);
  542. gps2_rtk->baseline_coords_type = mavlink_msg_gps2_rtk_get_baseline_coords_type(msg);
  543. #else
  544. uint8_t len = msg->len < MAVLINK_MSG_ID_GPS2_RTK_LEN? msg->len : MAVLINK_MSG_ID_GPS2_RTK_LEN;
  545. memset(gps2_rtk, 0, MAVLINK_MSG_ID_GPS2_RTK_LEN);
  546. memcpy(gps2_rtk, _MAV_PAYLOAD(msg), len);
  547. #endif
  548. }