mavlink_msg_fmub_status.h 32 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826
  1. #pragma once
  2. // MESSAGE FMUB_STATUS PACKING
  3. #define MAVLINK_MSG_ID_FMUB_STATUS 53200
  4. typedef struct __mavlink_fmub_status_t {
  5. uint32_t timestamp; /*< [ms] timestamp from systemboot in ms.*/
  6. float roll; /*< [rad] euler roll angle.*/
  7. float pitch; /*< [rad] euler pitch angle.*/
  8. float yaw; /*< [rad] euler yaw angle.*/
  9. float vn; /*< [m/s] velocity in northward axis.*/
  10. float ve; /*< [m/s] velocity in eastward axis.*/
  11. float vd; /*< [m/s] velocity in downward axis.*/
  12. float ax; /*< [m/s/s] acceleration in x axis.*/
  13. float ay; /*< [m/s/s] acceleration in y axis.*/
  14. float az; /*< [m/s/s] acceleration in z axis.*/
  15. float gx; /*< [rad/s] angular velocity in x axis.*/
  16. float gy; /*< [rad/s] angular velocity in y axis.*/
  17. float gz; /*< [rad/s] angular velocity in z axis.*/
  18. int32_t lat; /*< [degE7] latitude.*/
  19. int32_t lon; /*< [degE7] longitude.*/
  20. float amsl; /*< [m] altitude above mean sea level.*/
  21. int32_t ins_status; /*< */
  22. int32_t ins_flag; /*< */
  23. uint16_t throttle; /*< throttle output.*/
  24. uint8_t flight_mode; /*< flight mode.*/
  25. uint8_t btake_status; /*< backup takeover
  26. status.*/
  27. } mavlink_fmub_status_t;
  28. #define MAVLINK_MSG_ID_FMUB_STATUS_LEN 76
  29. #define MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN 76
  30. #define MAVLINK_MSG_ID_53200_LEN 76
  31. #define MAVLINK_MSG_ID_53200_MIN_LEN 76
  32. #define MAVLINK_MSG_ID_FMUB_STATUS_CRC 45
  33. #define MAVLINK_MSG_ID_53200_CRC 45
  34. #if MAVLINK_COMMAND_24BIT
  35. #define MAVLINK_MESSAGE_INFO_FMUB_STATUS { \
  36. 53200, \
  37. "FMUB_STATUS", \
  38. 21, \
  39. { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fmub_status_t, timestamp) }, \
  40. { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fmub_status_t, roll) }, \
  41. { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_fmub_status_t, pitch) }, \
  42. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_fmub_status_t, yaw) }, \
  43. { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_fmub_status_t, vn) }, \
  44. { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_fmub_status_t, ve) }, \
  45. { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_fmub_status_t, vd) }, \
  46. { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_fmub_status_t, ax) }, \
  47. { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_fmub_status_t, ay) }, \
  48. { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_fmub_status_t, az) }, \
  49. { "gx", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_fmub_status_t, gx) }, \
  50. { "gy", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_fmub_status_t, gy) }, \
  51. { "gz", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_fmub_status_t, gz) }, \
  52. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 52, offsetof(mavlink_fmub_status_t, lat) }, \
  53. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 56, offsetof(mavlink_fmub_status_t, lon) }, \
  54. { "amsl", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_fmub_status_t, amsl) }, \
  55. { "ins_status", NULL, MAVLINK_TYPE_INT32_T, 0, 64, offsetof(mavlink_fmub_status_t, ins_status) }, \
  56. { "ins_flag", NULL, MAVLINK_TYPE_INT32_T, 0, 68, offsetof(mavlink_fmub_status_t, ins_flag) }, \
  57. { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 72, offsetof(mavlink_fmub_status_t, throttle) }, \
  58. { "flight_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 74, offsetof(mavlink_fmub_status_t, flight_mode) }, \
  59. { "btake_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 75, offsetof(mavlink_fmub_status_t, btake_status) }, \
  60. } \
  61. }
  62. #else
  63. #define MAVLINK_MESSAGE_INFO_FMUB_STATUS { \
  64. "FMUB_STATUS", \
  65. 21, \
  66. { { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fmub_status_t, timestamp) }, \
  67. { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fmub_status_t, roll) }, \
  68. { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_fmub_status_t, pitch) }, \
  69. { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_fmub_status_t, yaw) }, \
  70. { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_fmub_status_t, vn) }, \
  71. { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_fmub_status_t, ve) }, \
  72. { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_fmub_status_t, vd) }, \
  73. { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_fmub_status_t, ax) }, \
  74. { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_fmub_status_t, ay) }, \
  75. { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_fmub_status_t, az) }, \
  76. { "gx", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_fmub_status_t, gx) }, \
  77. { "gy", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_fmub_status_t, gy) }, \
  78. { "gz", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_fmub_status_t, gz) }, \
  79. { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 52, offsetof(mavlink_fmub_status_t, lat) }, \
  80. { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 56, offsetof(mavlink_fmub_status_t, lon) }, \
  81. { "amsl", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_fmub_status_t, amsl) }, \
  82. { "ins_status", NULL, MAVLINK_TYPE_INT32_T, 0, 64, offsetof(mavlink_fmub_status_t, ins_status) }, \
  83. { "ins_flag", NULL, MAVLINK_TYPE_INT32_T, 0, 68, offsetof(mavlink_fmub_status_t, ins_flag) }, \
  84. { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 72, offsetof(mavlink_fmub_status_t, throttle) }, \
  85. { "flight_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 74, offsetof(mavlink_fmub_status_t, flight_mode) }, \
  86. { "btake_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 75, offsetof(mavlink_fmub_status_t, btake_status) }, \
  87. } \
  88. }
  89. #endif
  90. /**
  91. * @brief Pack a fmub_status message
  92. * @param system_id ID of this system
  93. * @param component_id ID of this component (e.g. 200 for IMU)
  94. * @param msg The MAVLink message to compress the data into
  95. *
  96. * @param timestamp [ms] timestamp from systemboot in ms.
  97. * @param roll [rad] euler roll angle.
  98. * @param pitch [rad] euler pitch angle.
  99. * @param yaw [rad] euler yaw angle.
  100. * @param vn [m/s] velocity in northward axis.
  101. * @param ve [m/s] velocity in eastward axis.
  102. * @param vd [m/s] velocity in downward axis.
  103. * @param ax [m/s/s] acceleration in x axis.
  104. * @param ay [m/s/s] acceleration in y axis.
  105. * @param az [m/s/s] acceleration in z axis.
  106. * @param gx [rad/s] angular velocity in x axis.
  107. * @param gy [rad/s] angular velocity in y axis.
  108. * @param gz [rad/s] angular velocity in z axis.
  109. * @param lat [degE7] latitude.
  110. * @param lon [degE7] longitude.
  111. * @param amsl [m] altitude above mean sea level.
  112. * @param ins_status
  113. * @param ins_flag
  114. * @param throttle throttle output.
  115. * @param flight_mode flight mode.
  116. * @param btake_status backup takeover
  117. status.
  118. * @return length of the message in bytes (excluding serial stream start sign)
  119. */
  120. static inline uint16_t mavlink_msg_fmub_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  121. uint32_t timestamp, float roll, float pitch, float yaw, float vn, float ve, float vd, float ax, float ay, float az, float gx, float gy, float gz, int32_t lat, int32_t lon, float amsl, int32_t ins_status, int32_t ins_flag, uint16_t throttle, uint8_t flight_mode, uint8_t btake_status)
  122. {
  123. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  124. char buf[MAVLINK_MSG_ID_FMUB_STATUS_LEN];
  125. _mav_put_uint32_t(buf, 0, timestamp);
  126. _mav_put_float(buf, 4, roll);
  127. _mav_put_float(buf, 8, pitch);
  128. _mav_put_float(buf, 12, yaw);
  129. _mav_put_float(buf, 16, vn);
  130. _mav_put_float(buf, 20, ve);
  131. _mav_put_float(buf, 24, vd);
  132. _mav_put_float(buf, 28, ax);
  133. _mav_put_float(buf, 32, ay);
  134. _mav_put_float(buf, 36, az);
  135. _mav_put_float(buf, 40, gx);
  136. _mav_put_float(buf, 44, gy);
  137. _mav_put_float(buf, 48, gz);
  138. _mav_put_int32_t(buf, 52, lat);
  139. _mav_put_int32_t(buf, 56, lon);
  140. _mav_put_float(buf, 60, amsl);
  141. _mav_put_int32_t(buf, 64, ins_status);
  142. _mav_put_int32_t(buf, 68, ins_flag);
  143. _mav_put_uint16_t(buf, 72, throttle);
  144. _mav_put_uint8_t(buf, 74, flight_mode);
  145. _mav_put_uint8_t(buf, 75, btake_status);
  146. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  147. #else
  148. mavlink_fmub_status_t packet;
  149. packet.timestamp = timestamp;
  150. packet.roll = roll;
  151. packet.pitch = pitch;
  152. packet.yaw = yaw;
  153. packet.vn = vn;
  154. packet.ve = ve;
  155. packet.vd = vd;
  156. packet.ax = ax;
  157. packet.ay = ay;
  158. packet.az = az;
  159. packet.gx = gx;
  160. packet.gy = gy;
  161. packet.gz = gz;
  162. packet.lat = lat;
  163. packet.lon = lon;
  164. packet.amsl = amsl;
  165. packet.ins_status = ins_status;
  166. packet.ins_flag = ins_flag;
  167. packet.throttle = throttle;
  168. packet.flight_mode = flight_mode;
  169. packet.btake_status = btake_status;
  170. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  171. #endif
  172. msg->msgid = MAVLINK_MSG_ID_FMUB_STATUS;
  173. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  174. }
  175. /**
  176. * @brief Pack a fmub_status message
  177. * @param system_id ID of this system
  178. * @param component_id ID of this component (e.g. 200 for IMU)
  179. * @param status MAVLink status structure
  180. * @param msg The MAVLink message to compress the data into
  181. *
  182. * @param timestamp [ms] timestamp from systemboot in ms.
  183. * @param roll [rad] euler roll angle.
  184. * @param pitch [rad] euler pitch angle.
  185. * @param yaw [rad] euler yaw angle.
  186. * @param vn [m/s] velocity in northward axis.
  187. * @param ve [m/s] velocity in eastward axis.
  188. * @param vd [m/s] velocity in downward axis.
  189. * @param ax [m/s/s] acceleration in x axis.
  190. * @param ay [m/s/s] acceleration in y axis.
  191. * @param az [m/s/s] acceleration in z axis.
  192. * @param gx [rad/s] angular velocity in x axis.
  193. * @param gy [rad/s] angular velocity in y axis.
  194. * @param gz [rad/s] angular velocity in z axis.
  195. * @param lat [degE7] latitude.
  196. * @param lon [degE7] longitude.
  197. * @param amsl [m] altitude above mean sea level.
  198. * @param ins_status
  199. * @param ins_flag
  200. * @param throttle throttle output.
  201. * @param flight_mode flight mode.
  202. * @param btake_status backup takeover
  203. status.
  204. * @return length of the message in bytes (excluding serial stream start sign)
  205. */
  206. static inline uint16_t mavlink_msg_fmub_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
  207. uint32_t timestamp, float roll, float pitch, float yaw, float vn, float ve, float vd, float ax, float ay, float az, float gx, float gy, float gz, int32_t lat, int32_t lon, float amsl, int32_t ins_status, int32_t ins_flag, uint16_t throttle, uint8_t flight_mode, uint8_t btake_status)
  208. {
  209. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  210. char buf[MAVLINK_MSG_ID_FMUB_STATUS_LEN];
  211. _mav_put_uint32_t(buf, 0, timestamp);
  212. _mav_put_float(buf, 4, roll);
  213. _mav_put_float(buf, 8, pitch);
  214. _mav_put_float(buf, 12, yaw);
  215. _mav_put_float(buf, 16, vn);
  216. _mav_put_float(buf, 20, ve);
  217. _mav_put_float(buf, 24, vd);
  218. _mav_put_float(buf, 28, ax);
  219. _mav_put_float(buf, 32, ay);
  220. _mav_put_float(buf, 36, az);
  221. _mav_put_float(buf, 40, gx);
  222. _mav_put_float(buf, 44, gy);
  223. _mav_put_float(buf, 48, gz);
  224. _mav_put_int32_t(buf, 52, lat);
  225. _mav_put_int32_t(buf, 56, lon);
  226. _mav_put_float(buf, 60, amsl);
  227. _mav_put_int32_t(buf, 64, ins_status);
  228. _mav_put_int32_t(buf, 68, ins_flag);
  229. _mav_put_uint16_t(buf, 72, throttle);
  230. _mav_put_uint8_t(buf, 74, flight_mode);
  231. _mav_put_uint8_t(buf, 75, btake_status);
  232. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  233. #else
  234. mavlink_fmub_status_t packet;
  235. packet.timestamp = timestamp;
  236. packet.roll = roll;
  237. packet.pitch = pitch;
  238. packet.yaw = yaw;
  239. packet.vn = vn;
  240. packet.ve = ve;
  241. packet.vd = vd;
  242. packet.ax = ax;
  243. packet.ay = ay;
  244. packet.az = az;
  245. packet.gx = gx;
  246. packet.gy = gy;
  247. packet.gz = gz;
  248. packet.lat = lat;
  249. packet.lon = lon;
  250. packet.amsl = amsl;
  251. packet.ins_status = ins_status;
  252. packet.ins_flag = ins_flag;
  253. packet.throttle = throttle;
  254. packet.flight_mode = flight_mode;
  255. packet.btake_status = btake_status;
  256. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  257. #endif
  258. msg->msgid = MAVLINK_MSG_ID_FMUB_STATUS;
  259. #if MAVLINK_CRC_EXTRA
  260. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  261. #else
  262. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  263. #endif
  264. }
  265. /**
  266. * @brief Pack a fmub_status message on a channel
  267. * @param system_id ID of this system
  268. * @param component_id ID of this component (e.g. 200 for IMU)
  269. * @param chan The MAVLink channel this message will be sent over
  270. * @param msg The MAVLink message to compress the data into
  271. * @param timestamp [ms] timestamp from systemboot in ms.
  272. * @param roll [rad] euler roll angle.
  273. * @param pitch [rad] euler pitch angle.
  274. * @param yaw [rad] euler yaw angle.
  275. * @param vn [m/s] velocity in northward axis.
  276. * @param ve [m/s] velocity in eastward axis.
  277. * @param vd [m/s] velocity in downward axis.
  278. * @param ax [m/s/s] acceleration in x axis.
  279. * @param ay [m/s/s] acceleration in y axis.
  280. * @param az [m/s/s] acceleration in z axis.
  281. * @param gx [rad/s] angular velocity in x axis.
  282. * @param gy [rad/s] angular velocity in y axis.
  283. * @param gz [rad/s] angular velocity in z axis.
  284. * @param lat [degE7] latitude.
  285. * @param lon [degE7] longitude.
  286. * @param amsl [m] altitude above mean sea level.
  287. * @param ins_status
  288. * @param ins_flag
  289. * @param throttle throttle output.
  290. * @param flight_mode flight mode.
  291. * @param btake_status backup takeover
  292. status.
  293. * @return length of the message in bytes (excluding serial stream start sign)
  294. */
  295. static inline uint16_t mavlink_msg_fmub_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  296. mavlink_message_t* msg,
  297. uint32_t timestamp,float roll,float pitch,float yaw,float vn,float ve,float vd,float ax,float ay,float az,float gx,float gy,float gz,int32_t lat,int32_t lon,float amsl,int32_t ins_status,int32_t ins_flag,uint16_t throttle,uint8_t flight_mode,uint8_t btake_status)
  298. {
  299. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  300. char buf[MAVLINK_MSG_ID_FMUB_STATUS_LEN];
  301. _mav_put_uint32_t(buf, 0, timestamp);
  302. _mav_put_float(buf, 4, roll);
  303. _mav_put_float(buf, 8, pitch);
  304. _mav_put_float(buf, 12, yaw);
  305. _mav_put_float(buf, 16, vn);
  306. _mav_put_float(buf, 20, ve);
  307. _mav_put_float(buf, 24, vd);
  308. _mav_put_float(buf, 28, ax);
  309. _mav_put_float(buf, 32, ay);
  310. _mav_put_float(buf, 36, az);
  311. _mav_put_float(buf, 40, gx);
  312. _mav_put_float(buf, 44, gy);
  313. _mav_put_float(buf, 48, gz);
  314. _mav_put_int32_t(buf, 52, lat);
  315. _mav_put_int32_t(buf, 56, lon);
  316. _mav_put_float(buf, 60, amsl);
  317. _mav_put_int32_t(buf, 64, ins_status);
  318. _mav_put_int32_t(buf, 68, ins_flag);
  319. _mav_put_uint16_t(buf, 72, throttle);
  320. _mav_put_uint8_t(buf, 74, flight_mode);
  321. _mav_put_uint8_t(buf, 75, btake_status);
  322. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  323. #else
  324. mavlink_fmub_status_t packet;
  325. packet.timestamp = timestamp;
  326. packet.roll = roll;
  327. packet.pitch = pitch;
  328. packet.yaw = yaw;
  329. packet.vn = vn;
  330. packet.ve = ve;
  331. packet.vd = vd;
  332. packet.ax = ax;
  333. packet.ay = ay;
  334. packet.az = az;
  335. packet.gx = gx;
  336. packet.gy = gy;
  337. packet.gz = gz;
  338. packet.lat = lat;
  339. packet.lon = lon;
  340. packet.amsl = amsl;
  341. packet.ins_status = ins_status;
  342. packet.ins_flag = ins_flag;
  343. packet.throttle = throttle;
  344. packet.flight_mode = flight_mode;
  345. packet.btake_status = btake_status;
  346. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  347. #endif
  348. msg->msgid = MAVLINK_MSG_ID_FMUB_STATUS;
  349. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  350. }
  351. /**
  352. * @brief Encode a fmub_status struct
  353. *
  354. * @param system_id ID of this system
  355. * @param component_id ID of this component (e.g. 200 for IMU)
  356. * @param msg The MAVLink message to compress the data into
  357. * @param fmub_status C-struct to read the message contents from
  358. */
  359. static inline uint16_t mavlink_msg_fmub_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fmub_status_t* fmub_status)
  360. {
  361. return mavlink_msg_fmub_status_pack(system_id, component_id, msg, fmub_status->timestamp, fmub_status->roll, fmub_status->pitch, fmub_status->yaw, fmub_status->vn, fmub_status->ve, fmub_status->vd, fmub_status->ax, fmub_status->ay, fmub_status->az, fmub_status->gx, fmub_status->gy, fmub_status->gz, fmub_status->lat, fmub_status->lon, fmub_status->amsl, fmub_status->ins_status, fmub_status->ins_flag, fmub_status->throttle, fmub_status->flight_mode, fmub_status->btake_status);
  362. }
  363. /**
  364. * @brief Encode a fmub_status struct on a channel
  365. *
  366. * @param system_id ID of this system
  367. * @param component_id ID of this component (e.g. 200 for IMU)
  368. * @param chan The MAVLink channel this message will be sent over
  369. * @param msg The MAVLink message to compress the data into
  370. * @param fmub_status C-struct to read the message contents from
  371. */
  372. static inline uint16_t mavlink_msg_fmub_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fmub_status_t* fmub_status)
  373. {
  374. return mavlink_msg_fmub_status_pack_chan(system_id, component_id, chan, msg, fmub_status->timestamp, fmub_status->roll, fmub_status->pitch, fmub_status->yaw, fmub_status->vn, fmub_status->ve, fmub_status->vd, fmub_status->ax, fmub_status->ay, fmub_status->az, fmub_status->gx, fmub_status->gy, fmub_status->gz, fmub_status->lat, fmub_status->lon, fmub_status->amsl, fmub_status->ins_status, fmub_status->ins_flag, fmub_status->throttle, fmub_status->flight_mode, fmub_status->btake_status);
  375. }
  376. /**
  377. * @brief Encode a fmub_status struct with provided status structure
  378. *
  379. * @param system_id ID of this system
  380. * @param component_id ID of this component (e.g. 200 for IMU)
  381. * @param status MAVLink status structure
  382. * @param msg The MAVLink message to compress the data into
  383. * @param fmub_status C-struct to read the message contents from
  384. */
  385. static inline uint16_t mavlink_msg_fmub_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_fmub_status_t* fmub_status)
  386. {
  387. return mavlink_msg_fmub_status_pack_status(system_id, component_id, _status, msg, fmub_status->timestamp, fmub_status->roll, fmub_status->pitch, fmub_status->yaw, fmub_status->vn, fmub_status->ve, fmub_status->vd, fmub_status->ax, fmub_status->ay, fmub_status->az, fmub_status->gx, fmub_status->gy, fmub_status->gz, fmub_status->lat, fmub_status->lon, fmub_status->amsl, fmub_status->ins_status, fmub_status->ins_flag, fmub_status->throttle, fmub_status->flight_mode, fmub_status->btake_status);
  388. }
  389. /**
  390. * @brief Send a fmub_status message
  391. * @param chan MAVLink channel to send the message
  392. *
  393. * @param timestamp [ms] timestamp from systemboot in ms.
  394. * @param roll [rad] euler roll angle.
  395. * @param pitch [rad] euler pitch angle.
  396. * @param yaw [rad] euler yaw angle.
  397. * @param vn [m/s] velocity in northward axis.
  398. * @param ve [m/s] velocity in eastward axis.
  399. * @param vd [m/s] velocity in downward axis.
  400. * @param ax [m/s/s] acceleration in x axis.
  401. * @param ay [m/s/s] acceleration in y axis.
  402. * @param az [m/s/s] acceleration in z axis.
  403. * @param gx [rad/s] angular velocity in x axis.
  404. * @param gy [rad/s] angular velocity in y axis.
  405. * @param gz [rad/s] angular velocity in z axis.
  406. * @param lat [degE7] latitude.
  407. * @param lon [degE7] longitude.
  408. * @param amsl [m] altitude above mean sea level.
  409. * @param ins_status
  410. * @param ins_flag
  411. * @param throttle throttle output.
  412. * @param flight_mode flight mode.
  413. * @param btake_status backup takeover
  414. status.
  415. */
  416. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  417. static inline void mavlink_msg_fmub_status_send(mavlink_channel_t chan, uint32_t timestamp, float roll, float pitch, float yaw, float vn, float ve, float vd, float ax, float ay, float az, float gx, float gy, float gz, int32_t lat, int32_t lon, float amsl, int32_t ins_status, int32_t ins_flag, uint16_t throttle, uint8_t flight_mode, uint8_t btake_status)
  418. {
  419. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  420. char buf[MAVLINK_MSG_ID_FMUB_STATUS_LEN];
  421. _mav_put_uint32_t(buf, 0, timestamp);
  422. _mav_put_float(buf, 4, roll);
  423. _mav_put_float(buf, 8, pitch);
  424. _mav_put_float(buf, 12, yaw);
  425. _mav_put_float(buf, 16, vn);
  426. _mav_put_float(buf, 20, ve);
  427. _mav_put_float(buf, 24, vd);
  428. _mav_put_float(buf, 28, ax);
  429. _mav_put_float(buf, 32, ay);
  430. _mav_put_float(buf, 36, az);
  431. _mav_put_float(buf, 40, gx);
  432. _mav_put_float(buf, 44, gy);
  433. _mav_put_float(buf, 48, gz);
  434. _mav_put_int32_t(buf, 52, lat);
  435. _mav_put_int32_t(buf, 56, lon);
  436. _mav_put_float(buf, 60, amsl);
  437. _mav_put_int32_t(buf, 64, ins_status);
  438. _mav_put_int32_t(buf, 68, ins_flag);
  439. _mav_put_uint16_t(buf, 72, throttle);
  440. _mav_put_uint8_t(buf, 74, flight_mode);
  441. _mav_put_uint8_t(buf, 75, btake_status);
  442. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FMUB_STATUS, buf, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  443. #else
  444. mavlink_fmub_status_t packet;
  445. packet.timestamp = timestamp;
  446. packet.roll = roll;
  447. packet.pitch = pitch;
  448. packet.yaw = yaw;
  449. packet.vn = vn;
  450. packet.ve = ve;
  451. packet.vd = vd;
  452. packet.ax = ax;
  453. packet.ay = ay;
  454. packet.az = az;
  455. packet.gx = gx;
  456. packet.gy = gy;
  457. packet.gz = gz;
  458. packet.lat = lat;
  459. packet.lon = lon;
  460. packet.amsl = amsl;
  461. packet.ins_status = ins_status;
  462. packet.ins_flag = ins_flag;
  463. packet.throttle = throttle;
  464. packet.flight_mode = flight_mode;
  465. packet.btake_status = btake_status;
  466. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FMUB_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  467. #endif
  468. }
  469. /**
  470. * @brief Send a fmub_status message
  471. * @param chan MAVLink channel to send the message
  472. * @param struct The MAVLink struct to serialize
  473. */
  474. static inline void mavlink_msg_fmub_status_send_struct(mavlink_channel_t chan, const mavlink_fmub_status_t* fmub_status)
  475. {
  476. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  477. mavlink_msg_fmub_status_send(chan, fmub_status->timestamp, fmub_status->roll, fmub_status->pitch, fmub_status->yaw, fmub_status->vn, fmub_status->ve, fmub_status->vd, fmub_status->ax, fmub_status->ay, fmub_status->az, fmub_status->gx, fmub_status->gy, fmub_status->gz, fmub_status->lat, fmub_status->lon, fmub_status->amsl, fmub_status->ins_status, fmub_status->ins_flag, fmub_status->throttle, fmub_status->flight_mode, fmub_status->btake_status);
  478. #else
  479. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FMUB_STATUS, (const char *)fmub_status, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  480. #endif
  481. }
  482. #if MAVLINK_MSG_ID_FMUB_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  483. /*
  484. This variant of _send() can be used to save stack space by re-using
  485. memory from the receive buffer. The caller provides a
  486. mavlink_message_t which is the size of a full mavlink message. This
  487. is usually the receive buffer for the channel, and allows a reply to an
  488. incoming message with minimum stack space usage.
  489. */
  490. static inline void mavlink_msg_fmub_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t timestamp, float roll, float pitch, float yaw, float vn, float ve, float vd, float ax, float ay, float az, float gx, float gy, float gz, int32_t lat, int32_t lon, float amsl, int32_t ins_status, int32_t ins_flag, uint16_t throttle, uint8_t flight_mode, uint8_t btake_status)
  491. {
  492. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  493. char *buf = (char *)msgbuf;
  494. _mav_put_uint32_t(buf, 0, timestamp);
  495. _mav_put_float(buf, 4, roll);
  496. _mav_put_float(buf, 8, pitch);
  497. _mav_put_float(buf, 12, yaw);
  498. _mav_put_float(buf, 16, vn);
  499. _mav_put_float(buf, 20, ve);
  500. _mav_put_float(buf, 24, vd);
  501. _mav_put_float(buf, 28, ax);
  502. _mav_put_float(buf, 32, ay);
  503. _mav_put_float(buf, 36, az);
  504. _mav_put_float(buf, 40, gx);
  505. _mav_put_float(buf, 44, gy);
  506. _mav_put_float(buf, 48, gz);
  507. _mav_put_int32_t(buf, 52, lat);
  508. _mav_put_int32_t(buf, 56, lon);
  509. _mav_put_float(buf, 60, amsl);
  510. _mav_put_int32_t(buf, 64, ins_status);
  511. _mav_put_int32_t(buf, 68, ins_flag);
  512. _mav_put_uint16_t(buf, 72, throttle);
  513. _mav_put_uint8_t(buf, 74, flight_mode);
  514. _mav_put_uint8_t(buf, 75, btake_status);
  515. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FMUB_STATUS, buf, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  516. #else
  517. mavlink_fmub_status_t *packet = (mavlink_fmub_status_t *)msgbuf;
  518. packet->timestamp = timestamp;
  519. packet->roll = roll;
  520. packet->pitch = pitch;
  521. packet->yaw = yaw;
  522. packet->vn = vn;
  523. packet->ve = ve;
  524. packet->vd = vd;
  525. packet->ax = ax;
  526. packet->ay = ay;
  527. packet->az = az;
  528. packet->gx = gx;
  529. packet->gy = gy;
  530. packet->gz = gz;
  531. packet->lat = lat;
  532. packet->lon = lon;
  533. packet->amsl = amsl;
  534. packet->ins_status = ins_status;
  535. packet->ins_flag = ins_flag;
  536. packet->throttle = throttle;
  537. packet->flight_mode = flight_mode;
  538. packet->btake_status = btake_status;
  539. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FMUB_STATUS, (const char *)packet, MAVLINK_MSG_ID_FMUB_STATUS_MIN_LEN, MAVLINK_MSG_ID_FMUB_STATUS_LEN, MAVLINK_MSG_ID_FMUB_STATUS_CRC);
  540. #endif
  541. }
  542. #endif
  543. #endif
  544. // MESSAGE FMUB_STATUS UNPACKING
  545. /**
  546. * @brief Get field timestamp from fmub_status message
  547. *
  548. * @return [ms] timestamp from systemboot in ms.
  549. */
  550. static inline uint32_t mavlink_msg_fmub_status_get_timestamp(const mavlink_message_t* msg)
  551. {
  552. return _MAV_RETURN_uint32_t(msg, 0);
  553. }
  554. /**
  555. * @brief Get field roll from fmub_status message
  556. *
  557. * @return [rad] euler roll angle.
  558. */
  559. static inline float mavlink_msg_fmub_status_get_roll(const mavlink_message_t* msg)
  560. {
  561. return _MAV_RETURN_float(msg, 4);
  562. }
  563. /**
  564. * @brief Get field pitch from fmub_status message
  565. *
  566. * @return [rad] euler pitch angle.
  567. */
  568. static inline float mavlink_msg_fmub_status_get_pitch(const mavlink_message_t* msg)
  569. {
  570. return _MAV_RETURN_float(msg, 8);
  571. }
  572. /**
  573. * @brief Get field yaw from fmub_status message
  574. *
  575. * @return [rad] euler yaw angle.
  576. */
  577. static inline float mavlink_msg_fmub_status_get_yaw(const mavlink_message_t* msg)
  578. {
  579. return _MAV_RETURN_float(msg, 12);
  580. }
  581. /**
  582. * @brief Get field vn from fmub_status message
  583. *
  584. * @return [m/s] velocity in northward axis.
  585. */
  586. static inline float mavlink_msg_fmub_status_get_vn(const mavlink_message_t* msg)
  587. {
  588. return _MAV_RETURN_float(msg, 16);
  589. }
  590. /**
  591. * @brief Get field ve from fmub_status message
  592. *
  593. * @return [m/s] velocity in eastward axis.
  594. */
  595. static inline float mavlink_msg_fmub_status_get_ve(const mavlink_message_t* msg)
  596. {
  597. return _MAV_RETURN_float(msg, 20);
  598. }
  599. /**
  600. * @brief Get field vd from fmub_status message
  601. *
  602. * @return [m/s] velocity in downward axis.
  603. */
  604. static inline float mavlink_msg_fmub_status_get_vd(const mavlink_message_t* msg)
  605. {
  606. return _MAV_RETURN_float(msg, 24);
  607. }
  608. /**
  609. * @brief Get field ax from fmub_status message
  610. *
  611. * @return [m/s/s] acceleration in x axis.
  612. */
  613. static inline float mavlink_msg_fmub_status_get_ax(const mavlink_message_t* msg)
  614. {
  615. return _MAV_RETURN_float(msg, 28);
  616. }
  617. /**
  618. * @brief Get field ay from fmub_status message
  619. *
  620. * @return [m/s/s] acceleration in y axis.
  621. */
  622. static inline float mavlink_msg_fmub_status_get_ay(const mavlink_message_t* msg)
  623. {
  624. return _MAV_RETURN_float(msg, 32);
  625. }
  626. /**
  627. * @brief Get field az from fmub_status message
  628. *
  629. * @return [m/s/s] acceleration in z axis.
  630. */
  631. static inline float mavlink_msg_fmub_status_get_az(const mavlink_message_t* msg)
  632. {
  633. return _MAV_RETURN_float(msg, 36);
  634. }
  635. /**
  636. * @brief Get field gx from fmub_status message
  637. *
  638. * @return [rad/s] angular velocity in x axis.
  639. */
  640. static inline float mavlink_msg_fmub_status_get_gx(const mavlink_message_t* msg)
  641. {
  642. return _MAV_RETURN_float(msg, 40);
  643. }
  644. /**
  645. * @brief Get field gy from fmub_status message
  646. *
  647. * @return [rad/s] angular velocity in y axis.
  648. */
  649. static inline float mavlink_msg_fmub_status_get_gy(const mavlink_message_t* msg)
  650. {
  651. return _MAV_RETURN_float(msg, 44);
  652. }
  653. /**
  654. * @brief Get field gz from fmub_status message
  655. *
  656. * @return [rad/s] angular velocity in z axis.
  657. */
  658. static inline float mavlink_msg_fmub_status_get_gz(const mavlink_message_t* msg)
  659. {
  660. return _MAV_RETURN_float(msg, 48);
  661. }
  662. /**
  663. * @brief Get field lat from fmub_status message
  664. *
  665. * @return [degE7] latitude.
  666. */
  667. static inline int32_t mavlink_msg_fmub_status_get_lat(const mavlink_message_t* msg)
  668. {
  669. return _MAV_RETURN_int32_t(msg, 52);
  670. }
  671. /**
  672. * @brief Get field lon from fmub_status message
  673. *
  674. * @return [degE7] longitude.
  675. */
  676. static inline int32_t mavlink_msg_fmub_status_get_lon(const mavlink_message_t* msg)
  677. {
  678. return _MAV_RETURN_int32_t(msg, 56);
  679. }
  680. /**
  681. * @brief Get field amsl from fmub_status message
  682. *
  683. * @return [m] altitude above mean sea level.
  684. */
  685. static inline float mavlink_msg_fmub_status_get_amsl(const mavlink_message_t* msg)
  686. {
  687. return _MAV_RETURN_float(msg, 60);
  688. }
  689. /**
  690. * @brief Get field ins_status from fmub_status message
  691. *
  692. * @return
  693. */
  694. static inline int32_t mavlink_msg_fmub_status_get_ins_status(const mavlink_message_t* msg)
  695. {
  696. return _MAV_RETURN_int32_t(msg, 64);
  697. }
  698. /**
  699. * @brief Get field ins_flag from fmub_status message
  700. *
  701. * @return
  702. */
  703. static inline int32_t mavlink_msg_fmub_status_get_ins_flag(const mavlink_message_t* msg)
  704. {
  705. return _MAV_RETURN_int32_t(msg, 68);
  706. }
  707. /**
  708. * @brief Get field throttle from fmub_status message
  709. *
  710. * @return throttle output.
  711. */
  712. static inline uint16_t mavlink_msg_fmub_status_get_throttle(const mavlink_message_t* msg)
  713. {
  714. return _MAV_RETURN_uint16_t(msg, 72);
  715. }
  716. /**
  717. * @brief Get field flight_mode from fmub_status message
  718. *
  719. * @return flight mode.
  720. */
  721. static inline uint8_t mavlink_msg_fmub_status_get_flight_mode(const mavlink_message_t* msg)
  722. {
  723. return _MAV_RETURN_uint8_t(msg, 74);
  724. }
  725. /**
  726. * @brief Get field btake_status from fmub_status message
  727. *
  728. * @return backup takeover
  729. status.
  730. */
  731. static inline uint8_t mavlink_msg_fmub_status_get_btake_status(const mavlink_message_t* msg)
  732. {
  733. return _MAV_RETURN_uint8_t(msg, 75);
  734. }
  735. /**
  736. * @brief Decode a fmub_status message into a struct
  737. *
  738. * @param msg The message to decode
  739. * @param fmub_status C-struct to decode the message contents into
  740. */
  741. static inline void mavlink_msg_fmub_status_decode(const mavlink_message_t* msg, mavlink_fmub_status_t* fmub_status)
  742. {
  743. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  744. fmub_status->timestamp = mavlink_msg_fmub_status_get_timestamp(msg);
  745. fmub_status->roll = mavlink_msg_fmub_status_get_roll(msg);
  746. fmub_status->pitch = mavlink_msg_fmub_status_get_pitch(msg);
  747. fmub_status->yaw = mavlink_msg_fmub_status_get_yaw(msg);
  748. fmub_status->vn = mavlink_msg_fmub_status_get_vn(msg);
  749. fmub_status->ve = mavlink_msg_fmub_status_get_ve(msg);
  750. fmub_status->vd = mavlink_msg_fmub_status_get_vd(msg);
  751. fmub_status->ax = mavlink_msg_fmub_status_get_ax(msg);
  752. fmub_status->ay = mavlink_msg_fmub_status_get_ay(msg);
  753. fmub_status->az = mavlink_msg_fmub_status_get_az(msg);
  754. fmub_status->gx = mavlink_msg_fmub_status_get_gx(msg);
  755. fmub_status->gy = mavlink_msg_fmub_status_get_gy(msg);
  756. fmub_status->gz = mavlink_msg_fmub_status_get_gz(msg);
  757. fmub_status->lat = mavlink_msg_fmub_status_get_lat(msg);
  758. fmub_status->lon = mavlink_msg_fmub_status_get_lon(msg);
  759. fmub_status->amsl = mavlink_msg_fmub_status_get_amsl(msg);
  760. fmub_status->ins_status = mavlink_msg_fmub_status_get_ins_status(msg);
  761. fmub_status->ins_flag = mavlink_msg_fmub_status_get_ins_flag(msg);
  762. fmub_status->throttle = mavlink_msg_fmub_status_get_throttle(msg);
  763. fmub_status->flight_mode = mavlink_msg_fmub_status_get_flight_mode(msg);
  764. fmub_status->btake_status = mavlink_msg_fmub_status_get_btake_status(msg);
  765. #else
  766. uint8_t len = msg->len < MAVLINK_MSG_ID_FMUB_STATUS_LEN? msg->len : MAVLINK_MSG_ID_FMUB_STATUS_LEN;
  767. memset(fmub_status, 0, MAVLINK_MSG_ID_FMUB_STATUS_LEN);
  768. memcpy(fmub_status, _MAV_PAYLOAD(msg), len);
  769. #endif
  770. }