mavlink_msg_hil_state.h 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588
  1. #pragma once
  2. // MESSAGE HIL_STATE PACKING
  3. #define MAVLINK_MSG_ID_HIL_STATE 90
  4. typedef struct __mavlink_hil_state_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. float roll; /*< [rad] Roll angle*/
  7. float pitch; /*< [rad] Pitch angle*/
  8. float yaw; /*< [rad] Yaw angle*/
  9. float rollspeed; /*< [rad/s] Body frame roll / phi angular speed*/
  10. float pitchspeed; /*< [rad/s] Body frame pitch / theta angular speed*/
  11. float yawspeed; /*< [rad/s] Body frame yaw / psi angular speed*/
  12. int32_t lat; /*< [degE7] Latitude*/
  13. int32_t lon; /*< [degE7] Longitude*/
  14. int32_t alt; /*< [mm] Altitude*/
  15. int16_t vx; /*< [cm/s] Ground X Speed (Latitude)*/
  16. int16_t vy; /*< [cm/s] Ground Y Speed (Longitude)*/
  17. int16_t vz; /*< [cm/s] Ground Z Speed (Altitude)*/
  18. int16_t xacc; /*< [mG] X acceleration*/
  19. int16_t yacc; /*< [mG] Y acceleration*/
  20. int16_t zacc; /*< [mG] Z acceleration*/
  21. } mavlink_hil_state_t;
  22. #define MAVLINK_MSG_ID_HIL_STATE_LEN 56
  23. #define MAVLINK_MSG_ID_HIL_STATE_MIN_LEN 56
  24. #define MAVLINK_MSG_ID_90_LEN 56
  25. #define MAVLINK_MSG_ID_90_MIN_LEN 56
  26. #define MAVLINK_MSG_ID_HIL_STATE_CRC 183
  27. #define MAVLINK_MSG_ID_90_CRC 183
  28. #if MAVLINK_COMMAND_24BIT
  29. #define MAVLINK_MESSAGE_INFO_HIL_STATE { \
  30. 90, \
  31. "HIL_STATE", \
  32. 16, \
  33. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, time_usec) }, \
  34. { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \
  35. { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \
  36. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \
  37. { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \
  38. { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \
  39. { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \
  40. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \
  41. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \
  42. { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \
  43. { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \
  44. { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \
  45. { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \
  46. { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \
  47. { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \
  48. { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \
  49. } \
  50. }
  51. #else
  52. #define MAVLINK_MESSAGE_INFO_HIL_STATE { \
  53. "HIL_STATE", \
  54. 16, \
  55. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, time_usec) }, \
  56. { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \
  57. { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \
  58. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \
  59. { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \
  60. { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \
  61. { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \
  62. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \
  63. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \
  64. { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \
  65. { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \
  66. { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \
  67. { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \
  68. { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \
  69. { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \
  70. { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \
  71. } \
  72. }
  73. #endif
  74. /**
  75. * @brief Pack a hil_state message
  76. * @param system_id ID of this system
  77. * @param component_id ID of this component (e.g. 200 for IMU)
  78. * @param msg The MAVLink message to compress the data into
  79. *
  80. * @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.
  81. * @param roll [rad] Roll angle
  82. * @param pitch [rad] Pitch angle
  83. * @param yaw [rad] Yaw angle
  84. * @param rollspeed [rad/s] Body frame roll / phi angular speed
  85. * @param pitchspeed [rad/s] Body frame pitch / theta angular speed
  86. * @param yawspeed [rad/s] Body frame yaw / psi angular speed
  87. * @param lat [degE7] Latitude
  88. * @param lon [degE7] Longitude
  89. * @param alt [mm] Altitude
  90. * @param vx [cm/s] Ground X Speed (Latitude)
  91. * @param vy [cm/s] Ground Y Speed (Longitude)
  92. * @param vz [cm/s] Ground Z Speed (Altitude)
  93. * @param xacc [mG] X acceleration
  94. * @param yacc [mG] Y acceleration
  95. * @param zacc [mG] Z acceleration
  96. * @return length of the message in bytes (excluding serial stream start sign)
  97. */
  98. static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  99. uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
  100. {
  101. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  102. char buf[MAVLINK_MSG_ID_HIL_STATE_LEN];
  103. _mav_put_uint64_t(buf, 0, time_usec);
  104. _mav_put_float(buf, 8, roll);
  105. _mav_put_float(buf, 12, pitch);
  106. _mav_put_float(buf, 16, yaw);
  107. _mav_put_float(buf, 20, rollspeed);
  108. _mav_put_float(buf, 24, pitchspeed);
  109. _mav_put_float(buf, 28, yawspeed);
  110. _mav_put_int32_t(buf, 32, lat);
  111. _mav_put_int32_t(buf, 36, lon);
  112. _mav_put_int32_t(buf, 40, alt);
  113. _mav_put_int16_t(buf, 44, vx);
  114. _mav_put_int16_t(buf, 46, vy);
  115. _mav_put_int16_t(buf, 48, vz);
  116. _mav_put_int16_t(buf, 50, xacc);
  117. _mav_put_int16_t(buf, 52, yacc);
  118. _mav_put_int16_t(buf, 54, zacc);
  119. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
  120. #else
  121. mavlink_hil_state_t packet;
  122. packet.time_usec = time_usec;
  123. packet.roll = roll;
  124. packet.pitch = pitch;
  125. packet.yaw = yaw;
  126. packet.rollspeed = rollspeed;
  127. packet.pitchspeed = pitchspeed;
  128. packet.yawspeed = yawspeed;
  129. packet.lat = lat;
  130. packet.lon = lon;
  131. packet.alt = alt;
  132. packet.vx = vx;
  133. packet.vy = vy;
  134. packet.vz = vz;
  135. packet.xacc = xacc;
  136. packet.yacc = yacc;
  137. packet.zacc = zacc;
  138. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
  139. #endif
  140. msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
  141. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  142. }
  143. /**
  144. * @brief Pack a hil_state message on a channel
  145. * @param system_id ID of this system
  146. * @param component_id ID of this component (e.g. 200 for IMU)
  147. * @param chan The MAVLink channel this message will be sent over
  148. * @param msg The MAVLink message to compress the data into
  149. * @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.
  150. * @param roll [rad] Roll angle
  151. * @param pitch [rad] Pitch angle
  152. * @param yaw [rad] Yaw angle
  153. * @param rollspeed [rad/s] Body frame roll / phi angular speed
  154. * @param pitchspeed [rad/s] Body frame pitch / theta angular speed
  155. * @param yawspeed [rad/s] Body frame yaw / psi angular speed
  156. * @param lat [degE7] Latitude
  157. * @param lon [degE7] Longitude
  158. * @param alt [mm] Altitude
  159. * @param vx [cm/s] Ground X Speed (Latitude)
  160. * @param vy [cm/s] Ground Y Speed (Longitude)
  161. * @param vz [cm/s] Ground Z Speed (Altitude)
  162. * @param xacc [mG] X acceleration
  163. * @param yacc [mG] Y acceleration
  164. * @param zacc [mG] Z acceleration
  165. * @return length of the message in bytes (excluding serial stream start sign)
  166. */
  167. static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  168. mavlink_message_t* msg,
  169. uint64_t time_usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,int16_t xacc,int16_t yacc,int16_t zacc)
  170. {
  171. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  172. char buf[MAVLINK_MSG_ID_HIL_STATE_LEN];
  173. _mav_put_uint64_t(buf, 0, time_usec);
  174. _mav_put_float(buf, 8, roll);
  175. _mav_put_float(buf, 12, pitch);
  176. _mav_put_float(buf, 16, yaw);
  177. _mav_put_float(buf, 20, rollspeed);
  178. _mav_put_float(buf, 24, pitchspeed);
  179. _mav_put_float(buf, 28, yawspeed);
  180. _mav_put_int32_t(buf, 32, lat);
  181. _mav_put_int32_t(buf, 36, lon);
  182. _mav_put_int32_t(buf, 40, alt);
  183. _mav_put_int16_t(buf, 44, vx);
  184. _mav_put_int16_t(buf, 46, vy);
  185. _mav_put_int16_t(buf, 48, vz);
  186. _mav_put_int16_t(buf, 50, xacc);
  187. _mav_put_int16_t(buf, 52, yacc);
  188. _mav_put_int16_t(buf, 54, zacc);
  189. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
  190. #else
  191. mavlink_hil_state_t packet;
  192. packet.time_usec = time_usec;
  193. packet.roll = roll;
  194. packet.pitch = pitch;
  195. packet.yaw = yaw;
  196. packet.rollspeed = rollspeed;
  197. packet.pitchspeed = pitchspeed;
  198. packet.yawspeed = yawspeed;
  199. packet.lat = lat;
  200. packet.lon = lon;
  201. packet.alt = alt;
  202. packet.vx = vx;
  203. packet.vy = vy;
  204. packet.vz = vz;
  205. packet.xacc = xacc;
  206. packet.yacc = yacc;
  207. packet.zacc = zacc;
  208. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
  209. #endif
  210. msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
  211. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  212. }
  213. /**
  214. * @brief Encode a hil_state struct
  215. *
  216. * @param system_id ID of this system
  217. * @param component_id ID of this component (e.g. 200 for IMU)
  218. * @param msg The MAVLink message to compress the data into
  219. * @param hil_state C-struct to read the message contents from
  220. */
  221. static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state)
  222. {
  223. return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
  224. }
  225. /**
  226. * @brief Encode a hil_state struct on a channel
  227. *
  228. * @param system_id ID of this system
  229. * @param component_id ID of this component (e.g. 200 for IMU)
  230. * @param chan The MAVLink channel this message will be sent over
  231. * @param msg The MAVLink message to compress the data into
  232. * @param hil_state C-struct to read the message contents from
  233. */
  234. static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state)
  235. {
  236. return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
  237. }
  238. /**
  239. * @brief Send a hil_state message
  240. * @param chan MAVLink channel to send the message
  241. *
  242. * @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.
  243. * @param roll [rad] Roll angle
  244. * @param pitch [rad] Pitch angle
  245. * @param yaw [rad] Yaw angle
  246. * @param rollspeed [rad/s] Body frame roll / phi angular speed
  247. * @param pitchspeed [rad/s] Body frame pitch / theta angular speed
  248. * @param yawspeed [rad/s] Body frame yaw / psi angular speed
  249. * @param lat [degE7] Latitude
  250. * @param lon [degE7] Longitude
  251. * @param alt [mm] Altitude
  252. * @param vx [cm/s] Ground X Speed (Latitude)
  253. * @param vy [cm/s] Ground Y Speed (Longitude)
  254. * @param vz [cm/s] Ground Z Speed (Altitude)
  255. * @param xacc [mG] X acceleration
  256. * @param yacc [mG] Y acceleration
  257. * @param zacc [mG] Z acceleration
  258. */
  259. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  260. static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
  261. {
  262. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  263. char buf[MAVLINK_MSG_ID_HIL_STATE_LEN];
  264. _mav_put_uint64_t(buf, 0, time_usec);
  265. _mav_put_float(buf, 8, roll);
  266. _mav_put_float(buf, 12, pitch);
  267. _mav_put_float(buf, 16, yaw);
  268. _mav_put_float(buf, 20, rollspeed);
  269. _mav_put_float(buf, 24, pitchspeed);
  270. _mav_put_float(buf, 28, yawspeed);
  271. _mav_put_int32_t(buf, 32, lat);
  272. _mav_put_int32_t(buf, 36, lon);
  273. _mav_put_int32_t(buf, 40, alt);
  274. _mav_put_int16_t(buf, 44, vx);
  275. _mav_put_int16_t(buf, 46, vy);
  276. _mav_put_int16_t(buf, 48, vz);
  277. _mav_put_int16_t(buf, 50, xacc);
  278. _mav_put_int16_t(buf, 52, yacc);
  279. _mav_put_int16_t(buf, 54, zacc);
  280. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  281. #else
  282. mavlink_hil_state_t packet;
  283. packet.time_usec = time_usec;
  284. packet.roll = roll;
  285. packet.pitch = pitch;
  286. packet.yaw = yaw;
  287. packet.rollspeed = rollspeed;
  288. packet.pitchspeed = pitchspeed;
  289. packet.yawspeed = yawspeed;
  290. packet.lat = lat;
  291. packet.lon = lon;
  292. packet.alt = alt;
  293. packet.vx = vx;
  294. packet.vy = vy;
  295. packet.vz = vz;
  296. packet.xacc = xacc;
  297. packet.yacc = yacc;
  298. packet.zacc = zacc;
  299. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  300. #endif
  301. }
  302. /**
  303. * @brief Send a hil_state message
  304. * @param chan MAVLink channel to send the message
  305. * @param struct The MAVLink struct to serialize
  306. */
  307. static inline void mavlink_msg_hil_state_send_struct(mavlink_channel_t chan, const mavlink_hil_state_t* hil_state)
  308. {
  309. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  310. mavlink_msg_hil_state_send(chan, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
  311. #else
  312. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)hil_state, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  313. #endif
  314. }
  315. #if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  316. /*
  317. This variant of _send() can be used to save stack space by re-using
  318. memory from the receive buffer. The caller provides a
  319. mavlink_message_t which is the size of a full mavlink message. This
  320. is usually the receive buffer for the channel, and allows a reply to an
  321. incoming message with minimum stack space usage.
  322. */
  323. static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
  324. {
  325. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  326. char *buf = (char *)msgbuf;
  327. _mav_put_uint64_t(buf, 0, time_usec);
  328. _mav_put_float(buf, 8, roll);
  329. _mav_put_float(buf, 12, pitch);
  330. _mav_put_float(buf, 16, yaw);
  331. _mav_put_float(buf, 20, rollspeed);
  332. _mav_put_float(buf, 24, pitchspeed);
  333. _mav_put_float(buf, 28, yawspeed);
  334. _mav_put_int32_t(buf, 32, lat);
  335. _mav_put_int32_t(buf, 36, lon);
  336. _mav_put_int32_t(buf, 40, alt);
  337. _mav_put_int16_t(buf, 44, vx);
  338. _mav_put_int16_t(buf, 46, vy);
  339. _mav_put_int16_t(buf, 48, vz);
  340. _mav_put_int16_t(buf, 50, xacc);
  341. _mav_put_int16_t(buf, 52, yacc);
  342. _mav_put_int16_t(buf, 54, zacc);
  343. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  344. #else
  345. mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf;
  346. packet->time_usec = time_usec;
  347. packet->roll = roll;
  348. packet->pitch = pitch;
  349. packet->yaw = yaw;
  350. packet->rollspeed = rollspeed;
  351. packet->pitchspeed = pitchspeed;
  352. packet->yawspeed = yawspeed;
  353. packet->lat = lat;
  354. packet->lon = lon;
  355. packet->alt = alt;
  356. packet->vx = vx;
  357. packet->vy = vy;
  358. packet->vz = vz;
  359. packet->xacc = xacc;
  360. packet->yacc = yacc;
  361. packet->zacc = zacc;
  362. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
  363. #endif
  364. }
  365. #endif
  366. #endif
  367. // MESSAGE HIL_STATE UNPACKING
  368. /**
  369. * @brief Get field time_usec from hil_state message
  370. *
  371. * @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.
  372. */
  373. static inline uint64_t mavlink_msg_hil_state_get_time_usec(const mavlink_message_t* msg)
  374. {
  375. return _MAV_RETURN_uint64_t(msg, 0);
  376. }
  377. /**
  378. * @brief Get field roll from hil_state message
  379. *
  380. * @return [rad] Roll angle
  381. */
  382. static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg)
  383. {
  384. return _MAV_RETURN_float(msg, 8);
  385. }
  386. /**
  387. * @brief Get field pitch from hil_state message
  388. *
  389. * @return [rad] Pitch angle
  390. */
  391. static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg)
  392. {
  393. return _MAV_RETURN_float(msg, 12);
  394. }
  395. /**
  396. * @brief Get field yaw from hil_state message
  397. *
  398. * @return [rad] Yaw angle
  399. */
  400. static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg)
  401. {
  402. return _MAV_RETURN_float(msg, 16);
  403. }
  404. /**
  405. * @brief Get field rollspeed from hil_state message
  406. *
  407. * @return [rad/s] Body frame roll / phi angular speed
  408. */
  409. static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg)
  410. {
  411. return _MAV_RETURN_float(msg, 20);
  412. }
  413. /**
  414. * @brief Get field pitchspeed from hil_state message
  415. *
  416. * @return [rad/s] Body frame pitch / theta angular speed
  417. */
  418. static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg)
  419. {
  420. return _MAV_RETURN_float(msg, 24);
  421. }
  422. /**
  423. * @brief Get field yawspeed from hil_state message
  424. *
  425. * @return [rad/s] Body frame yaw / psi angular speed
  426. */
  427. static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg)
  428. {
  429. return _MAV_RETURN_float(msg, 28);
  430. }
  431. /**
  432. * @brief Get field lat from hil_state message
  433. *
  434. * @return [degE7] Latitude
  435. */
  436. static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg)
  437. {
  438. return _MAV_RETURN_int32_t(msg, 32);
  439. }
  440. /**
  441. * @brief Get field lon from hil_state message
  442. *
  443. * @return [degE7] Longitude
  444. */
  445. static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg)
  446. {
  447. return _MAV_RETURN_int32_t(msg, 36);
  448. }
  449. /**
  450. * @brief Get field alt from hil_state message
  451. *
  452. * @return [mm] Altitude
  453. */
  454. static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg)
  455. {
  456. return _MAV_RETURN_int32_t(msg, 40);
  457. }
  458. /**
  459. * @brief Get field vx from hil_state message
  460. *
  461. * @return [cm/s] Ground X Speed (Latitude)
  462. */
  463. static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg)
  464. {
  465. return _MAV_RETURN_int16_t(msg, 44);
  466. }
  467. /**
  468. * @brief Get field vy from hil_state message
  469. *
  470. * @return [cm/s] Ground Y Speed (Longitude)
  471. */
  472. static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg)
  473. {
  474. return _MAV_RETURN_int16_t(msg, 46);
  475. }
  476. /**
  477. * @brief Get field vz from hil_state message
  478. *
  479. * @return [cm/s] Ground Z Speed (Altitude)
  480. */
  481. static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg)
  482. {
  483. return _MAV_RETURN_int16_t(msg, 48);
  484. }
  485. /**
  486. * @brief Get field xacc from hil_state message
  487. *
  488. * @return [mG] X acceleration
  489. */
  490. static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg)
  491. {
  492. return _MAV_RETURN_int16_t(msg, 50);
  493. }
  494. /**
  495. * @brief Get field yacc from hil_state message
  496. *
  497. * @return [mG] Y acceleration
  498. */
  499. static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg)
  500. {
  501. return _MAV_RETURN_int16_t(msg, 52);
  502. }
  503. /**
  504. * @brief Get field zacc from hil_state message
  505. *
  506. * @return [mG] Z acceleration
  507. */
  508. static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg)
  509. {
  510. return _MAV_RETURN_int16_t(msg, 54);
  511. }
  512. /**
  513. * @brief Decode a hil_state message into a struct
  514. *
  515. * @param msg The message to decode
  516. * @param hil_state C-struct to decode the message contents into
  517. */
  518. static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state)
  519. {
  520. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  521. hil_state->time_usec = mavlink_msg_hil_state_get_time_usec(msg);
  522. hil_state->roll = mavlink_msg_hil_state_get_roll(msg);
  523. hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg);
  524. hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg);
  525. hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg);
  526. hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg);
  527. hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg);
  528. hil_state->lat = mavlink_msg_hil_state_get_lat(msg);
  529. hil_state->lon = mavlink_msg_hil_state_get_lon(msg);
  530. hil_state->alt = mavlink_msg_hil_state_get_alt(msg);
  531. hil_state->vx = mavlink_msg_hil_state_get_vx(msg);
  532. hil_state->vy = mavlink_msg_hil_state_get_vy(msg);
  533. hil_state->vz = mavlink_msg_hil_state_get_vz(msg);
  534. hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg);
  535. hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg);
  536. hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg);
  537. #else
  538. uint8_t len = msg->len < MAVLINK_MSG_ID_HIL_STATE_LEN? msg->len : MAVLINK_MSG_ID_HIL_STATE_LEN;
  539. memset(hil_state, 0, MAVLINK_MSG_ID_HIL_STATE_LEN);
  540. memcpy(hil_state, _MAV_PAYLOAD(msg), len);
  541. #endif
  542. }