mavlink_msg_sim_state.h 32 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763
  1. #pragma once
  2. // MESSAGE SIM_STATE PACKING
  3. #define MAVLINK_MSG_ID_SIM_STATE 108
  4. typedef struct __mavlink_sim_state_t {
  5. float q1; /*< True attitude quaternion component 1, w (1 in null-rotation)*/
  6. float q2; /*< True attitude quaternion component 2, x (0 in null-rotation)*/
  7. float q3; /*< True attitude quaternion component 3, y (0 in null-rotation)*/
  8. float q4; /*< True attitude quaternion component 4, z (0 in null-rotation)*/
  9. float roll; /*< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs*/
  10. float pitch; /*< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs*/
  11. float yaw; /*< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs*/
  12. float xacc; /*< [m/s/s] X acceleration*/
  13. float yacc; /*< [m/s/s] Y acceleration*/
  14. float zacc; /*< [m/s/s] Z acceleration*/
  15. float xgyro; /*< [rad/s] Angular speed around X axis*/
  16. float ygyro; /*< [rad/s] Angular speed around Y axis*/
  17. float zgyro; /*< [rad/s] Angular speed around Z axis*/
  18. float lat; /*< [deg] Latitude (lower precision). Both this and the lat_int field should be set.*/
  19. float lon; /*< [deg] Longitude (lower precision). Both this and the lon_int field should be set.*/
  20. float alt; /*< [m] Altitude*/
  21. float std_dev_horz; /*< Horizontal position standard deviation*/
  22. float std_dev_vert; /*< Vertical position standard deviation*/
  23. float vn; /*< [m/s] True velocity in north direction in earth-fixed NED frame*/
  24. float ve; /*< [m/s] True velocity in east direction in earth-fixed NED frame*/
  25. float vd; /*< [m/s] True velocity in down direction in earth-fixed NED frame*/
  26. int32_t lat_int; /*< [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred).*/
  27. int32_t lon_int; /*< [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred).*/
  28. } mavlink_sim_state_t;
  29. #define MAVLINK_MSG_ID_SIM_STATE_LEN 92
  30. #define MAVLINK_MSG_ID_SIM_STATE_MIN_LEN 84
  31. #define MAVLINK_MSG_ID_108_LEN 92
  32. #define MAVLINK_MSG_ID_108_MIN_LEN 84
  33. #define MAVLINK_MSG_ID_SIM_STATE_CRC 32
  34. #define MAVLINK_MSG_ID_108_CRC 32
  35. #if MAVLINK_COMMAND_24BIT
  36. #define MAVLINK_MESSAGE_INFO_SIM_STATE { \
  37. 108, \
  38. "SIM_STATE", \
  39. 23, \
  40. { { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \
  41. { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \
  42. { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \
  43. { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sim_state_t, q4) }, \
  44. { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sim_state_t, roll) }, \
  45. { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sim_state_t, pitch) }, \
  46. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sim_state_t, yaw) }, \
  47. { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sim_state_t, xacc) }, \
  48. { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sim_state_t, yacc) }, \
  49. { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_sim_state_t, zacc) }, \
  50. { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_sim_state_t, xgyro) }, \
  51. { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_sim_state_t, ygyro) }, \
  52. { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_sim_state_t, zgyro) }, \
  53. { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_sim_state_t, lat) }, \
  54. { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_sim_state_t, lon) }, \
  55. { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_sim_state_t, alt) }, \
  56. { "std_dev_horz", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_sim_state_t, std_dev_horz) }, \
  57. { "std_dev_vert", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_sim_state_t, std_dev_vert) }, \
  58. { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \
  59. { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \
  60. { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \
  61. { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 84, offsetof(mavlink_sim_state_t, lat_int) }, \
  62. { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 88, offsetof(mavlink_sim_state_t, lon_int) }, \
  63. } \
  64. }
  65. #else
  66. #define MAVLINK_MESSAGE_INFO_SIM_STATE { \
  67. "SIM_STATE", \
  68. 23, \
  69. { { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \
  70. { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \
  71. { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \
  72. { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sim_state_t, q4) }, \
  73. { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sim_state_t, roll) }, \
  74. { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sim_state_t, pitch) }, \
  75. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sim_state_t, yaw) }, \
  76. { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sim_state_t, xacc) }, \
  77. { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sim_state_t, yacc) }, \
  78. { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_sim_state_t, zacc) }, \
  79. { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_sim_state_t, xgyro) }, \
  80. { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_sim_state_t, ygyro) }, \
  81. { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_sim_state_t, zgyro) }, \
  82. { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_sim_state_t, lat) }, \
  83. { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_sim_state_t, lon) }, \
  84. { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_sim_state_t, alt) }, \
  85. { "std_dev_horz", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_sim_state_t, std_dev_horz) }, \
  86. { "std_dev_vert", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_sim_state_t, std_dev_vert) }, \
  87. { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \
  88. { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \
  89. { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \
  90. { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 84, offsetof(mavlink_sim_state_t, lat_int) }, \
  91. { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 88, offsetof(mavlink_sim_state_t, lon_int) }, \
  92. } \
  93. }
  94. #endif
  95. /**
  96. * @brief Pack a sim_state message
  97. * @param system_id ID of this system
  98. * @param component_id ID of this component (e.g. 200 for IMU)
  99. * @param msg The MAVLink message to compress the data into
  100. *
  101. * @param q1 True attitude quaternion component 1, w (1 in null-rotation)
  102. * @param q2 True attitude quaternion component 2, x (0 in null-rotation)
  103. * @param q3 True attitude quaternion component 3, y (0 in null-rotation)
  104. * @param q4 True attitude quaternion component 4, z (0 in null-rotation)
  105. * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
  106. * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
  107. * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
  108. * @param xacc [m/s/s] X acceleration
  109. * @param yacc [m/s/s] Y acceleration
  110. * @param zacc [m/s/s] Z acceleration
  111. * @param xgyro [rad/s] Angular speed around X axis
  112. * @param ygyro [rad/s] Angular speed around Y axis
  113. * @param zgyro [rad/s] Angular speed around Z axis
  114. * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set.
  115. * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set.
  116. * @param alt [m] Altitude
  117. * @param std_dev_horz Horizontal position standard deviation
  118. * @param std_dev_vert Vertical position standard deviation
  119. * @param vn [m/s] True velocity in north direction in earth-fixed NED frame
  120. * @param ve [m/s] True velocity in east direction in earth-fixed NED frame
  121. * @param vd [m/s] True velocity in down direction in earth-fixed NED frame
  122. * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred).
  123. * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred).
  124. * @return length of the message in bytes (excluding serial stream start sign)
  125. */
  126. static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  127. float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int)
  128. {
  129. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  130. char buf[MAVLINK_MSG_ID_SIM_STATE_LEN];
  131. _mav_put_float(buf, 0, q1);
  132. _mav_put_float(buf, 4, q2);
  133. _mav_put_float(buf, 8, q3);
  134. _mav_put_float(buf, 12, q4);
  135. _mav_put_float(buf, 16, roll);
  136. _mav_put_float(buf, 20, pitch);
  137. _mav_put_float(buf, 24, yaw);
  138. _mav_put_float(buf, 28, xacc);
  139. _mav_put_float(buf, 32, yacc);
  140. _mav_put_float(buf, 36, zacc);
  141. _mav_put_float(buf, 40, xgyro);
  142. _mav_put_float(buf, 44, ygyro);
  143. _mav_put_float(buf, 48, zgyro);
  144. _mav_put_float(buf, 52, lat);
  145. _mav_put_float(buf, 56, lon);
  146. _mav_put_float(buf, 60, alt);
  147. _mav_put_float(buf, 64, std_dev_horz);
  148. _mav_put_float(buf, 68, std_dev_vert);
  149. _mav_put_float(buf, 72, vn);
  150. _mav_put_float(buf, 76, ve);
  151. _mav_put_float(buf, 80, vd);
  152. _mav_put_int32_t(buf, 84, lat_int);
  153. _mav_put_int32_t(buf, 88, lon_int);
  154. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN);
  155. #else
  156. mavlink_sim_state_t packet;
  157. packet.q1 = q1;
  158. packet.q2 = q2;
  159. packet.q3 = q3;
  160. packet.q4 = q4;
  161. packet.roll = roll;
  162. packet.pitch = pitch;
  163. packet.yaw = yaw;
  164. packet.xacc = xacc;
  165. packet.yacc = yacc;
  166. packet.zacc = zacc;
  167. packet.xgyro = xgyro;
  168. packet.ygyro = ygyro;
  169. packet.zgyro = zgyro;
  170. packet.lat = lat;
  171. packet.lon = lon;
  172. packet.alt = alt;
  173. packet.std_dev_horz = std_dev_horz;
  174. packet.std_dev_vert = std_dev_vert;
  175. packet.vn = vn;
  176. packet.ve = ve;
  177. packet.vd = vd;
  178. packet.lat_int = lat_int;
  179. packet.lon_int = lon_int;
  180. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN);
  181. #endif
  182. msg->msgid = MAVLINK_MSG_ID_SIM_STATE;
  183. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  184. }
  185. /**
  186. * @brief Pack a sim_state message on a channel
  187. * @param system_id ID of this system
  188. * @param component_id ID of this component (e.g. 200 for IMU)
  189. * @param chan The MAVLink channel this message will be sent over
  190. * @param msg The MAVLink message to compress the data into
  191. * @param q1 True attitude quaternion component 1, w (1 in null-rotation)
  192. * @param q2 True attitude quaternion component 2, x (0 in null-rotation)
  193. * @param q3 True attitude quaternion component 3, y (0 in null-rotation)
  194. * @param q4 True attitude quaternion component 4, z (0 in null-rotation)
  195. * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
  196. * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
  197. * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
  198. * @param xacc [m/s/s] X acceleration
  199. * @param yacc [m/s/s] Y acceleration
  200. * @param zacc [m/s/s] Z acceleration
  201. * @param xgyro [rad/s] Angular speed around X axis
  202. * @param ygyro [rad/s] Angular speed around Y axis
  203. * @param zgyro [rad/s] Angular speed around Z axis
  204. * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set.
  205. * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set.
  206. * @param alt [m] Altitude
  207. * @param std_dev_horz Horizontal position standard deviation
  208. * @param std_dev_vert Vertical position standard deviation
  209. * @param vn [m/s] True velocity in north direction in earth-fixed NED frame
  210. * @param ve [m/s] True velocity in east direction in earth-fixed NED frame
  211. * @param vd [m/s] True velocity in down direction in earth-fixed NED frame
  212. * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred).
  213. * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred).
  214. * @return length of the message in bytes (excluding serial stream start sign)
  215. */
  216. static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  217. mavlink_message_t* msg,
  218. float q1,float q2,float q3,float q4,float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lon,float alt,float std_dev_horz,float std_dev_vert,float vn,float ve,float vd,int32_t lat_int,int32_t lon_int)
  219. {
  220. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  221. char buf[MAVLINK_MSG_ID_SIM_STATE_LEN];
  222. _mav_put_float(buf, 0, q1);
  223. _mav_put_float(buf, 4, q2);
  224. _mav_put_float(buf, 8, q3);
  225. _mav_put_float(buf, 12, q4);
  226. _mav_put_float(buf, 16, roll);
  227. _mav_put_float(buf, 20, pitch);
  228. _mav_put_float(buf, 24, yaw);
  229. _mav_put_float(buf, 28, xacc);
  230. _mav_put_float(buf, 32, yacc);
  231. _mav_put_float(buf, 36, zacc);
  232. _mav_put_float(buf, 40, xgyro);
  233. _mav_put_float(buf, 44, ygyro);
  234. _mav_put_float(buf, 48, zgyro);
  235. _mav_put_float(buf, 52, lat);
  236. _mav_put_float(buf, 56, lon);
  237. _mav_put_float(buf, 60, alt);
  238. _mav_put_float(buf, 64, std_dev_horz);
  239. _mav_put_float(buf, 68, std_dev_vert);
  240. _mav_put_float(buf, 72, vn);
  241. _mav_put_float(buf, 76, ve);
  242. _mav_put_float(buf, 80, vd);
  243. _mav_put_int32_t(buf, 84, lat_int);
  244. _mav_put_int32_t(buf, 88, lon_int);
  245. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN);
  246. #else
  247. mavlink_sim_state_t packet;
  248. packet.q1 = q1;
  249. packet.q2 = q2;
  250. packet.q3 = q3;
  251. packet.q4 = q4;
  252. packet.roll = roll;
  253. packet.pitch = pitch;
  254. packet.yaw = yaw;
  255. packet.xacc = xacc;
  256. packet.yacc = yacc;
  257. packet.zacc = zacc;
  258. packet.xgyro = xgyro;
  259. packet.ygyro = ygyro;
  260. packet.zgyro = zgyro;
  261. packet.lat = lat;
  262. packet.lon = lon;
  263. packet.alt = alt;
  264. packet.std_dev_horz = std_dev_horz;
  265. packet.std_dev_vert = std_dev_vert;
  266. packet.vn = vn;
  267. packet.ve = ve;
  268. packet.vd = vd;
  269. packet.lat_int = lat_int;
  270. packet.lon_int = lon_int;
  271. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN);
  272. #endif
  273. msg->msgid = MAVLINK_MSG_ID_SIM_STATE;
  274. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  275. }
  276. /**
  277. * @brief Encode a sim_state struct
  278. *
  279. * @param system_id ID of this system
  280. * @param component_id ID of this component (e.g. 200 for IMU)
  281. * @param msg The MAVLink message to compress the data into
  282. * @param sim_state C-struct to read the message contents from
  283. */
  284. static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state)
  285. {
  286. return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int);
  287. }
  288. /**
  289. * @brief Encode a sim_state struct on a channel
  290. *
  291. * @param system_id ID of this system
  292. * @param component_id ID of this component (e.g. 200 for IMU)
  293. * @param chan The MAVLink channel this message will be sent over
  294. * @param msg The MAVLink message to compress the data into
  295. * @param sim_state C-struct to read the message contents from
  296. */
  297. static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state)
  298. {
  299. return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int);
  300. }
  301. /**
  302. * @brief Send a sim_state message
  303. * @param chan MAVLink channel to send the message
  304. *
  305. * @param q1 True attitude quaternion component 1, w (1 in null-rotation)
  306. * @param q2 True attitude quaternion component 2, x (0 in null-rotation)
  307. * @param q3 True attitude quaternion component 3, y (0 in null-rotation)
  308. * @param q4 True attitude quaternion component 4, z (0 in null-rotation)
  309. * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
  310. * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
  311. * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
  312. * @param xacc [m/s/s] X acceleration
  313. * @param yacc [m/s/s] Y acceleration
  314. * @param zacc [m/s/s] Z acceleration
  315. * @param xgyro [rad/s] Angular speed around X axis
  316. * @param ygyro [rad/s] Angular speed around Y axis
  317. * @param zgyro [rad/s] Angular speed around Z axis
  318. * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set.
  319. * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set.
  320. * @param alt [m] Altitude
  321. * @param std_dev_horz Horizontal position standard deviation
  322. * @param std_dev_vert Vertical position standard deviation
  323. * @param vn [m/s] True velocity in north direction in earth-fixed NED frame
  324. * @param ve [m/s] True velocity in east direction in earth-fixed NED frame
  325. * @param vd [m/s] True velocity in down direction in earth-fixed NED frame
  326. * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred).
  327. * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred).
  328. */
  329. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  330. static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int)
  331. {
  332. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  333. char buf[MAVLINK_MSG_ID_SIM_STATE_LEN];
  334. _mav_put_float(buf, 0, q1);
  335. _mav_put_float(buf, 4, q2);
  336. _mav_put_float(buf, 8, q3);
  337. _mav_put_float(buf, 12, q4);
  338. _mav_put_float(buf, 16, roll);
  339. _mav_put_float(buf, 20, pitch);
  340. _mav_put_float(buf, 24, yaw);
  341. _mav_put_float(buf, 28, xacc);
  342. _mav_put_float(buf, 32, yacc);
  343. _mav_put_float(buf, 36, zacc);
  344. _mav_put_float(buf, 40, xgyro);
  345. _mav_put_float(buf, 44, ygyro);
  346. _mav_put_float(buf, 48, zgyro);
  347. _mav_put_float(buf, 52, lat);
  348. _mav_put_float(buf, 56, lon);
  349. _mav_put_float(buf, 60, alt);
  350. _mav_put_float(buf, 64, std_dev_horz);
  351. _mav_put_float(buf, 68, std_dev_vert);
  352. _mav_put_float(buf, 72, vn);
  353. _mav_put_float(buf, 76, ve);
  354. _mav_put_float(buf, 80, vd);
  355. _mav_put_int32_t(buf, 84, lat_int);
  356. _mav_put_int32_t(buf, 88, lon_int);
  357. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  358. #else
  359. mavlink_sim_state_t packet;
  360. packet.q1 = q1;
  361. packet.q2 = q2;
  362. packet.q3 = q3;
  363. packet.q4 = q4;
  364. packet.roll = roll;
  365. packet.pitch = pitch;
  366. packet.yaw = yaw;
  367. packet.xacc = xacc;
  368. packet.yacc = yacc;
  369. packet.zacc = zacc;
  370. packet.xgyro = xgyro;
  371. packet.ygyro = ygyro;
  372. packet.zgyro = zgyro;
  373. packet.lat = lat;
  374. packet.lon = lon;
  375. packet.alt = alt;
  376. packet.std_dev_horz = std_dev_horz;
  377. packet.std_dev_vert = std_dev_vert;
  378. packet.vn = vn;
  379. packet.ve = ve;
  380. packet.vd = vd;
  381. packet.lat_int = lat_int;
  382. packet.lon_int = lon_int;
  383. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)&packet, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  384. #endif
  385. }
  386. /**
  387. * @brief Send a sim_state message
  388. * @param chan MAVLink channel to send the message
  389. * @param struct The MAVLink struct to serialize
  390. */
  391. static inline void mavlink_msg_sim_state_send_struct(mavlink_channel_t chan, const mavlink_sim_state_t* sim_state)
  392. {
  393. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  394. mavlink_msg_sim_state_send(chan, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int);
  395. #else
  396. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)sim_state, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  397. #endif
  398. }
  399. #if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  400. /*
  401. This variant of _send() can be used to save stack space by re-using
  402. memory from the receive buffer. The caller provides a
  403. mavlink_message_t which is the size of a full mavlink message. This
  404. is usually the receive buffer for the channel, and allows a reply to an
  405. incoming message with minimum stack space usage.
  406. */
  407. static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int)
  408. {
  409. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  410. char *buf = (char *)msgbuf;
  411. _mav_put_float(buf, 0, q1);
  412. _mav_put_float(buf, 4, q2);
  413. _mav_put_float(buf, 8, q3);
  414. _mav_put_float(buf, 12, q4);
  415. _mav_put_float(buf, 16, roll);
  416. _mav_put_float(buf, 20, pitch);
  417. _mav_put_float(buf, 24, yaw);
  418. _mav_put_float(buf, 28, xacc);
  419. _mav_put_float(buf, 32, yacc);
  420. _mav_put_float(buf, 36, zacc);
  421. _mav_put_float(buf, 40, xgyro);
  422. _mav_put_float(buf, 44, ygyro);
  423. _mav_put_float(buf, 48, zgyro);
  424. _mav_put_float(buf, 52, lat);
  425. _mav_put_float(buf, 56, lon);
  426. _mav_put_float(buf, 60, alt);
  427. _mav_put_float(buf, 64, std_dev_horz);
  428. _mav_put_float(buf, 68, std_dev_vert);
  429. _mav_put_float(buf, 72, vn);
  430. _mav_put_float(buf, 76, ve);
  431. _mav_put_float(buf, 80, vd);
  432. _mav_put_int32_t(buf, 84, lat_int);
  433. _mav_put_int32_t(buf, 88, lon_int);
  434. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  435. #else
  436. mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf;
  437. packet->q1 = q1;
  438. packet->q2 = q2;
  439. packet->q3 = q3;
  440. packet->q4 = q4;
  441. packet->roll = roll;
  442. packet->pitch = pitch;
  443. packet->yaw = yaw;
  444. packet->xacc = xacc;
  445. packet->yacc = yacc;
  446. packet->zacc = zacc;
  447. packet->xgyro = xgyro;
  448. packet->ygyro = ygyro;
  449. packet->zgyro = zgyro;
  450. packet->lat = lat;
  451. packet->lon = lon;
  452. packet->alt = alt;
  453. packet->std_dev_horz = std_dev_horz;
  454. packet->std_dev_vert = std_dev_vert;
  455. packet->vn = vn;
  456. packet->ve = ve;
  457. packet->vd = vd;
  458. packet->lat_int = lat_int;
  459. packet->lon_int = lon_int;
  460. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
  461. #endif
  462. }
  463. #endif
  464. #endif
  465. // MESSAGE SIM_STATE UNPACKING
  466. /**
  467. * @brief Get field q1 from sim_state message
  468. *
  469. * @return True attitude quaternion component 1, w (1 in null-rotation)
  470. */
  471. static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg)
  472. {
  473. return _MAV_RETURN_float(msg, 0);
  474. }
  475. /**
  476. * @brief Get field q2 from sim_state message
  477. *
  478. * @return True attitude quaternion component 2, x (0 in null-rotation)
  479. */
  480. static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg)
  481. {
  482. return _MAV_RETURN_float(msg, 4);
  483. }
  484. /**
  485. * @brief Get field q3 from sim_state message
  486. *
  487. * @return True attitude quaternion component 3, y (0 in null-rotation)
  488. */
  489. static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg)
  490. {
  491. return _MAV_RETURN_float(msg, 8);
  492. }
  493. /**
  494. * @brief Get field q4 from sim_state message
  495. *
  496. * @return True attitude quaternion component 4, z (0 in null-rotation)
  497. */
  498. static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg)
  499. {
  500. return _MAV_RETURN_float(msg, 12);
  501. }
  502. /**
  503. * @brief Get field roll from sim_state message
  504. *
  505. * @return Attitude roll expressed as Euler angles, not recommended except for human-readable outputs
  506. */
  507. static inline float mavlink_msg_sim_state_get_roll(const mavlink_message_t* msg)
  508. {
  509. return _MAV_RETURN_float(msg, 16);
  510. }
  511. /**
  512. * @brief Get field pitch from sim_state message
  513. *
  514. * @return Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs
  515. */
  516. static inline float mavlink_msg_sim_state_get_pitch(const mavlink_message_t* msg)
  517. {
  518. return _MAV_RETURN_float(msg, 20);
  519. }
  520. /**
  521. * @brief Get field yaw from sim_state message
  522. *
  523. * @return Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs
  524. */
  525. static inline float mavlink_msg_sim_state_get_yaw(const mavlink_message_t* msg)
  526. {
  527. return _MAV_RETURN_float(msg, 24);
  528. }
  529. /**
  530. * @brief Get field xacc from sim_state message
  531. *
  532. * @return [m/s/s] X acceleration
  533. */
  534. static inline float mavlink_msg_sim_state_get_xacc(const mavlink_message_t* msg)
  535. {
  536. return _MAV_RETURN_float(msg, 28);
  537. }
  538. /**
  539. * @brief Get field yacc from sim_state message
  540. *
  541. * @return [m/s/s] Y acceleration
  542. */
  543. static inline float mavlink_msg_sim_state_get_yacc(const mavlink_message_t* msg)
  544. {
  545. return _MAV_RETURN_float(msg, 32);
  546. }
  547. /**
  548. * @brief Get field zacc from sim_state message
  549. *
  550. * @return [m/s/s] Z acceleration
  551. */
  552. static inline float mavlink_msg_sim_state_get_zacc(const mavlink_message_t* msg)
  553. {
  554. return _MAV_RETURN_float(msg, 36);
  555. }
  556. /**
  557. * @brief Get field xgyro from sim_state message
  558. *
  559. * @return [rad/s] Angular speed around X axis
  560. */
  561. static inline float mavlink_msg_sim_state_get_xgyro(const mavlink_message_t* msg)
  562. {
  563. return _MAV_RETURN_float(msg, 40);
  564. }
  565. /**
  566. * @brief Get field ygyro from sim_state message
  567. *
  568. * @return [rad/s] Angular speed around Y axis
  569. */
  570. static inline float mavlink_msg_sim_state_get_ygyro(const mavlink_message_t* msg)
  571. {
  572. return _MAV_RETURN_float(msg, 44);
  573. }
  574. /**
  575. * @brief Get field zgyro from sim_state message
  576. *
  577. * @return [rad/s] Angular speed around Z axis
  578. */
  579. static inline float mavlink_msg_sim_state_get_zgyro(const mavlink_message_t* msg)
  580. {
  581. return _MAV_RETURN_float(msg, 48);
  582. }
  583. /**
  584. * @brief Get field lat from sim_state message
  585. *
  586. * @return [deg] Latitude (lower precision). Both this and the lat_int field should be set.
  587. */
  588. static inline float mavlink_msg_sim_state_get_lat(const mavlink_message_t* msg)
  589. {
  590. return _MAV_RETURN_float(msg, 52);
  591. }
  592. /**
  593. * @brief Get field lon from sim_state message
  594. *
  595. * @return [deg] Longitude (lower precision). Both this and the lon_int field should be set.
  596. */
  597. static inline float mavlink_msg_sim_state_get_lon(const mavlink_message_t* msg)
  598. {
  599. return _MAV_RETURN_float(msg, 56);
  600. }
  601. /**
  602. * @brief Get field alt from sim_state message
  603. *
  604. * @return [m] Altitude
  605. */
  606. static inline float mavlink_msg_sim_state_get_alt(const mavlink_message_t* msg)
  607. {
  608. return _MAV_RETURN_float(msg, 60);
  609. }
  610. /**
  611. * @brief Get field std_dev_horz from sim_state message
  612. *
  613. * @return Horizontal position standard deviation
  614. */
  615. static inline float mavlink_msg_sim_state_get_std_dev_horz(const mavlink_message_t* msg)
  616. {
  617. return _MAV_RETURN_float(msg, 64);
  618. }
  619. /**
  620. * @brief Get field std_dev_vert from sim_state message
  621. *
  622. * @return Vertical position standard deviation
  623. */
  624. static inline float mavlink_msg_sim_state_get_std_dev_vert(const mavlink_message_t* msg)
  625. {
  626. return _MAV_RETURN_float(msg, 68);
  627. }
  628. /**
  629. * @brief Get field vn from sim_state message
  630. *
  631. * @return [m/s] True velocity in north direction in earth-fixed NED frame
  632. */
  633. static inline float mavlink_msg_sim_state_get_vn(const mavlink_message_t* msg)
  634. {
  635. return _MAV_RETURN_float(msg, 72);
  636. }
  637. /**
  638. * @brief Get field ve from sim_state message
  639. *
  640. * @return [m/s] True velocity in east direction in earth-fixed NED frame
  641. */
  642. static inline float mavlink_msg_sim_state_get_ve(const mavlink_message_t* msg)
  643. {
  644. return _MAV_RETURN_float(msg, 76);
  645. }
  646. /**
  647. * @brief Get field vd from sim_state message
  648. *
  649. * @return [m/s] True velocity in down direction in earth-fixed NED frame
  650. */
  651. static inline float mavlink_msg_sim_state_get_vd(const mavlink_message_t* msg)
  652. {
  653. return _MAV_RETURN_float(msg, 80);
  654. }
  655. /**
  656. * @brief Get field lat_int from sim_state message
  657. *
  658. * @return [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred).
  659. */
  660. static inline int32_t mavlink_msg_sim_state_get_lat_int(const mavlink_message_t* msg)
  661. {
  662. return _MAV_RETURN_int32_t(msg, 84);
  663. }
  664. /**
  665. * @brief Get field lon_int from sim_state message
  666. *
  667. * @return [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred).
  668. */
  669. static inline int32_t mavlink_msg_sim_state_get_lon_int(const mavlink_message_t* msg)
  670. {
  671. return _MAV_RETURN_int32_t(msg, 88);
  672. }
  673. /**
  674. * @brief Decode a sim_state message into a struct
  675. *
  676. * @param msg The message to decode
  677. * @param sim_state C-struct to decode the message contents into
  678. */
  679. static inline void mavlink_msg_sim_state_decode(const mavlink_message_t* msg, mavlink_sim_state_t* sim_state)
  680. {
  681. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  682. sim_state->q1 = mavlink_msg_sim_state_get_q1(msg);
  683. sim_state->q2 = mavlink_msg_sim_state_get_q2(msg);
  684. sim_state->q3 = mavlink_msg_sim_state_get_q3(msg);
  685. sim_state->q4 = mavlink_msg_sim_state_get_q4(msg);
  686. sim_state->roll = mavlink_msg_sim_state_get_roll(msg);
  687. sim_state->pitch = mavlink_msg_sim_state_get_pitch(msg);
  688. sim_state->yaw = mavlink_msg_sim_state_get_yaw(msg);
  689. sim_state->xacc = mavlink_msg_sim_state_get_xacc(msg);
  690. sim_state->yacc = mavlink_msg_sim_state_get_yacc(msg);
  691. sim_state->zacc = mavlink_msg_sim_state_get_zacc(msg);
  692. sim_state->xgyro = mavlink_msg_sim_state_get_xgyro(msg);
  693. sim_state->ygyro = mavlink_msg_sim_state_get_ygyro(msg);
  694. sim_state->zgyro = mavlink_msg_sim_state_get_zgyro(msg);
  695. sim_state->lat = mavlink_msg_sim_state_get_lat(msg);
  696. sim_state->lon = mavlink_msg_sim_state_get_lon(msg);
  697. sim_state->alt = mavlink_msg_sim_state_get_alt(msg);
  698. sim_state->std_dev_horz = mavlink_msg_sim_state_get_std_dev_horz(msg);
  699. sim_state->std_dev_vert = mavlink_msg_sim_state_get_std_dev_vert(msg);
  700. sim_state->vn = mavlink_msg_sim_state_get_vn(msg);
  701. sim_state->ve = mavlink_msg_sim_state_get_ve(msg);
  702. sim_state->vd = mavlink_msg_sim_state_get_vd(msg);
  703. sim_state->lat_int = mavlink_msg_sim_state_get_lat_int(msg);
  704. sim_state->lon_int = mavlink_msg_sim_state_get_lon_int(msg);
  705. #else
  706. uint8_t len = msg->len < MAVLINK_MSG_ID_SIM_STATE_LEN? msg->len : MAVLINK_MSG_ID_SIM_STATE_LEN;
  707. memset(sim_state, 0, MAVLINK_MSG_ID_SIM_STATE_LEN);
  708. memcpy(sim_state, _MAV_PAYLOAD(msg), len);
  709. #endif
  710. }