mavlink_msg_set_attitude_target.h 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431
  1. #pragma once
  2. // MESSAGE SET_ATTITUDE_TARGET PACKING
  3. #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET 82
  4. MAVPACKED(
  5. typedef struct __mavlink_set_attitude_target_t {
  6. uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/
  7. float q[4]; /*< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD*/
  8. float body_roll_rate; /*< [rad/s] Body roll rate*/
  9. float body_pitch_rate; /*< [rad/s] Body pitch rate*/
  10. float body_yaw_rate; /*< [rad/s] Body yaw rate*/
  11. float thrust; /*< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/
  12. uint8_t target_system; /*< System ID*/
  13. uint8_t target_component; /*< Component ID*/
  14. uint8_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/
  15. float thrust_body[3]; /*< 3D thrust setpoint in the body NED frame, normalized to -1 .. 1*/
  16. }) mavlink_set_attitude_target_t;
  17. #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 51
  18. #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN 39
  19. #define MAVLINK_MSG_ID_82_LEN 51
  20. #define MAVLINK_MSG_ID_82_MIN_LEN 39
  21. #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC 49
  22. #define MAVLINK_MSG_ID_82_CRC 49
  23. #define MAVLINK_MSG_SET_ATTITUDE_TARGET_FIELD_Q_LEN 4
  24. #define MAVLINK_MSG_SET_ATTITUDE_TARGET_FIELD_THRUST_BODY_LEN 3
  25. #if MAVLINK_COMMAND_24BIT
  26. #define MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET { \
  27. 82, \
  28. "SET_ATTITUDE_TARGET", \
  29. 10, \
  30. { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_attitude_target_t, time_boot_ms) }, \
  31. { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_set_attitude_target_t, target_system) }, \
  32. { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_set_attitude_target_t, target_component) }, \
  33. { "type_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_set_attitude_target_t, type_mask) }, \
  34. { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_set_attitude_target_t, q) }, \
  35. { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_attitude_target_t, body_roll_rate) }, \
  36. { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_attitude_target_t, body_pitch_rate) }, \
  37. { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_attitude_target_t, body_yaw_rate) }, \
  38. { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_attitude_target_t, thrust) }, \
  39. { "thrust_body", NULL, MAVLINK_TYPE_FLOAT, 3, 39, offsetof(mavlink_set_attitude_target_t, thrust_body) }, \
  40. } \
  41. }
  42. #else
  43. #define MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET { \
  44. "SET_ATTITUDE_TARGET", \
  45. 10, \
  46. { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_attitude_target_t, time_boot_ms) }, \
  47. { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_set_attitude_target_t, target_system) }, \
  48. { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_set_attitude_target_t, target_component) }, \
  49. { "type_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_set_attitude_target_t, type_mask) }, \
  50. { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_set_attitude_target_t, q) }, \
  51. { "body_roll_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_set_attitude_target_t, body_roll_rate) }, \
  52. { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_attitude_target_t, body_pitch_rate) }, \
  53. { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_attitude_target_t, body_yaw_rate) }, \
  54. { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_attitude_target_t, thrust) }, \
  55. { "thrust_body", NULL, MAVLINK_TYPE_FLOAT, 3, 39, offsetof(mavlink_set_attitude_target_t, thrust_body) }, \
  56. } \
  57. }
  58. #endif
  59. /**
  60. * @brief Pack a set_attitude_target message
  61. * @param system_id ID of this system
  62. * @param component_id ID of this component (e.g. 200 for IMU)
  63. * @param msg The MAVLink message to compress the data into
  64. *
  65. * @param time_boot_ms [ms] Timestamp (time since system boot).
  66. * @param target_system System ID
  67. * @param target_component Component ID
  68. * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle.
  69. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD
  70. * @param body_roll_rate [rad/s] Body roll rate
  71. * @param body_pitch_rate [rad/s] Body pitch rate
  72. * @param body_yaw_rate [rad/s] Body yaw rate
  73. * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)
  74. * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1
  75. * @return length of the message in bytes (excluding serial stream start sign)
  76. */
  77. static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  78. uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body)
  79. {
  80. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  81. char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN];
  82. _mav_put_uint32_t(buf, 0, time_boot_ms);
  83. _mav_put_float(buf, 20, body_roll_rate);
  84. _mav_put_float(buf, 24, body_pitch_rate);
  85. _mav_put_float(buf, 28, body_yaw_rate);
  86. _mav_put_float(buf, 32, thrust);
  87. _mav_put_uint8_t(buf, 36, target_system);
  88. _mav_put_uint8_t(buf, 37, target_component);
  89. _mav_put_uint8_t(buf, 38, type_mask);
  90. _mav_put_float_array(buf, 4, q, 4);
  91. _mav_put_float_array(buf, 39, thrust_body, 3);
  92. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN);
  93. #else
  94. mavlink_set_attitude_target_t packet;
  95. packet.time_boot_ms = time_boot_ms;
  96. packet.body_roll_rate = body_roll_rate;
  97. packet.body_pitch_rate = body_pitch_rate;
  98. packet.body_yaw_rate = body_yaw_rate;
  99. packet.thrust = thrust;
  100. packet.target_system = target_system;
  101. packet.target_component = target_component;
  102. packet.type_mask = type_mask;
  103. mav_array_memcpy(packet.q, q, sizeof(float)*4);
  104. mav_array_memcpy(packet.thrust_body, thrust_body, sizeof(float)*3);
  105. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN);
  106. #endif
  107. msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET;
  108. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  109. }
  110. /**
  111. * @brief Pack a set_attitude_target message on a channel
  112. * @param system_id ID of this system
  113. * @param component_id ID of this component (e.g. 200 for IMU)
  114. * @param chan The MAVLink channel this message will be sent over
  115. * @param msg The MAVLink message to compress the data into
  116. * @param time_boot_ms [ms] Timestamp (time since system boot).
  117. * @param target_system System ID
  118. * @param target_component Component ID
  119. * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle.
  120. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD
  121. * @param body_roll_rate [rad/s] Body roll rate
  122. * @param body_pitch_rate [rad/s] Body pitch rate
  123. * @param body_yaw_rate [rad/s] Body yaw rate
  124. * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)
  125. * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1
  126. * @return length of the message in bytes (excluding serial stream start sign)
  127. */
  128. static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  129. mavlink_message_t* msg,
  130. uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust,const float *thrust_body)
  131. {
  132. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  133. char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN];
  134. _mav_put_uint32_t(buf, 0, time_boot_ms);
  135. _mav_put_float(buf, 20, body_roll_rate);
  136. _mav_put_float(buf, 24, body_pitch_rate);
  137. _mav_put_float(buf, 28, body_yaw_rate);
  138. _mav_put_float(buf, 32, thrust);
  139. _mav_put_uint8_t(buf, 36, target_system);
  140. _mav_put_uint8_t(buf, 37, target_component);
  141. _mav_put_uint8_t(buf, 38, type_mask);
  142. _mav_put_float_array(buf, 4, q, 4);
  143. _mav_put_float_array(buf, 39, thrust_body, 3);
  144. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN);
  145. #else
  146. mavlink_set_attitude_target_t packet;
  147. packet.time_boot_ms = time_boot_ms;
  148. packet.body_roll_rate = body_roll_rate;
  149. packet.body_pitch_rate = body_pitch_rate;
  150. packet.body_yaw_rate = body_yaw_rate;
  151. packet.thrust = thrust;
  152. packet.target_system = target_system;
  153. packet.target_component = target_component;
  154. packet.type_mask = type_mask;
  155. mav_array_memcpy(packet.q, q, sizeof(float)*4);
  156. mav_array_memcpy(packet.thrust_body, thrust_body, sizeof(float)*3);
  157. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN);
  158. #endif
  159. msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET;
  160. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  161. }
  162. /**
  163. * @brief Encode a set_attitude_target struct
  164. *
  165. * @param system_id ID of this system
  166. * @param component_id ID of this component (e.g. 200 for IMU)
  167. * @param msg The MAVLink message to compress the data into
  168. * @param set_attitude_target C-struct to read the message contents from
  169. */
  170. static inline uint16_t mavlink_msg_set_attitude_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target)
  171. {
  172. return mavlink_msg_set_attitude_target_pack(system_id, component_id, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body);
  173. }
  174. /**
  175. * @brief Encode a set_attitude_target struct on a channel
  176. *
  177. * @param system_id ID of this system
  178. * @param component_id ID of this component (e.g. 200 for IMU)
  179. * @param chan The MAVLink channel this message will be sent over
  180. * @param msg The MAVLink message to compress the data into
  181. * @param set_attitude_target C-struct to read the message contents from
  182. */
  183. static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target)
  184. {
  185. return mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, chan, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body);
  186. }
  187. /**
  188. * @brief Send a set_attitude_target message
  189. * @param chan MAVLink channel to send the message
  190. *
  191. * @param time_boot_ms [ms] Timestamp (time since system boot).
  192. * @param target_system System ID
  193. * @param target_component Component ID
  194. * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle.
  195. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD
  196. * @param body_roll_rate [rad/s] Body roll rate
  197. * @param body_pitch_rate [rad/s] Body pitch rate
  198. * @param body_yaw_rate [rad/s] Body yaw rate
  199. * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)
  200. * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1
  201. */
  202. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  203. static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body)
  204. {
  205. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  206. char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN];
  207. _mav_put_uint32_t(buf, 0, time_boot_ms);
  208. _mav_put_float(buf, 20, body_roll_rate);
  209. _mav_put_float(buf, 24, body_pitch_rate);
  210. _mav_put_float(buf, 28, body_yaw_rate);
  211. _mav_put_float(buf, 32, thrust);
  212. _mav_put_uint8_t(buf, 36, target_system);
  213. _mav_put_uint8_t(buf, 37, target_component);
  214. _mav_put_uint8_t(buf, 38, type_mask);
  215. _mav_put_float_array(buf, 4, q, 4);
  216. _mav_put_float_array(buf, 39, thrust_body, 3);
  217. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  218. #else
  219. mavlink_set_attitude_target_t packet;
  220. packet.time_boot_ms = time_boot_ms;
  221. packet.body_roll_rate = body_roll_rate;
  222. packet.body_pitch_rate = body_pitch_rate;
  223. packet.body_yaw_rate = body_yaw_rate;
  224. packet.thrust = thrust;
  225. packet.target_system = target_system;
  226. packet.target_component = target_component;
  227. packet.type_mask = type_mask;
  228. mav_array_memcpy(packet.q, q, sizeof(float)*4);
  229. mav_array_memcpy(packet.thrust_body, thrust_body, sizeof(float)*3);
  230. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  231. #endif
  232. }
  233. /**
  234. * @brief Send a set_attitude_target message
  235. * @param chan MAVLink channel to send the message
  236. * @param struct The MAVLink struct to serialize
  237. */
  238. static inline void mavlink_msg_set_attitude_target_send_struct(mavlink_channel_t chan, const mavlink_set_attitude_target_t* set_attitude_target)
  239. {
  240. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  241. mavlink_msg_set_attitude_target_send(chan, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body);
  242. #else
  243. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)set_attitude_target, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  244. #endif
  245. }
  246. #if MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  247. /*
  248. This variant of _send() can be used to save stack space by re-using
  249. memory from the receive buffer. The caller provides a
  250. mavlink_message_t which is the size of a full mavlink message. This
  251. is usually the receive buffer for the channel, and allows a reply to an
  252. incoming message with minimum stack space usage.
  253. */
  254. static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body)
  255. {
  256. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  257. char *buf = (char *)msgbuf;
  258. _mav_put_uint32_t(buf, 0, time_boot_ms);
  259. _mav_put_float(buf, 20, body_roll_rate);
  260. _mav_put_float(buf, 24, body_pitch_rate);
  261. _mav_put_float(buf, 28, body_yaw_rate);
  262. _mav_put_float(buf, 32, thrust);
  263. _mav_put_uint8_t(buf, 36, target_system);
  264. _mav_put_uint8_t(buf, 37, target_component);
  265. _mav_put_uint8_t(buf, 38, type_mask);
  266. _mav_put_float_array(buf, 4, q, 4);
  267. _mav_put_float_array(buf, 39, thrust_body, 3);
  268. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  269. #else
  270. mavlink_set_attitude_target_t *packet = (mavlink_set_attitude_target_t *)msgbuf;
  271. packet->time_boot_ms = time_boot_ms;
  272. packet->body_roll_rate = body_roll_rate;
  273. packet->body_pitch_rate = body_pitch_rate;
  274. packet->body_yaw_rate = body_yaw_rate;
  275. packet->thrust = thrust;
  276. packet->target_system = target_system;
  277. packet->target_component = target_component;
  278. packet->type_mask = type_mask;
  279. mav_array_memcpy(packet->q, q, sizeof(float)*4);
  280. mav_array_memcpy(packet->thrust_body, thrust_body, sizeof(float)*3);
  281. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC);
  282. #endif
  283. }
  284. #endif
  285. #endif
  286. // MESSAGE SET_ATTITUDE_TARGET UNPACKING
  287. /**
  288. * @brief Get field time_boot_ms from set_attitude_target message
  289. *
  290. * @return [ms] Timestamp (time since system boot).
  291. */
  292. static inline uint32_t mavlink_msg_set_attitude_target_get_time_boot_ms(const mavlink_message_t* msg)
  293. {
  294. return _MAV_RETURN_uint32_t(msg, 0);
  295. }
  296. /**
  297. * @brief Get field target_system from set_attitude_target message
  298. *
  299. * @return System ID
  300. */
  301. static inline uint8_t mavlink_msg_set_attitude_target_get_target_system(const mavlink_message_t* msg)
  302. {
  303. return _MAV_RETURN_uint8_t(msg, 36);
  304. }
  305. /**
  306. * @brief Get field target_component from set_attitude_target message
  307. *
  308. * @return Component ID
  309. */
  310. static inline uint8_t mavlink_msg_set_attitude_target_get_target_component(const mavlink_message_t* msg)
  311. {
  312. return _MAV_RETURN_uint8_t(msg, 37);
  313. }
  314. /**
  315. * @brief Get field type_mask from set_attitude_target message
  316. *
  317. * @return Bitmap to indicate which dimensions should be ignored by the vehicle.
  318. */
  319. static inline uint8_t mavlink_msg_set_attitude_target_get_type_mask(const mavlink_message_t* msg)
  320. {
  321. return _MAV_RETURN_uint8_t(msg, 38);
  322. }
  323. /**
  324. * @brief Get field q from set_attitude_target message
  325. *
  326. * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD
  327. */
  328. static inline uint16_t mavlink_msg_set_attitude_target_get_q(const mavlink_message_t* msg, float *q)
  329. {
  330. return _MAV_RETURN_float_array(msg, q, 4, 4);
  331. }
  332. /**
  333. * @brief Get field body_roll_rate from set_attitude_target message
  334. *
  335. * @return [rad/s] Body roll rate
  336. */
  337. static inline float mavlink_msg_set_attitude_target_get_body_roll_rate(const mavlink_message_t* msg)
  338. {
  339. return _MAV_RETURN_float(msg, 20);
  340. }
  341. /**
  342. * @brief Get field body_pitch_rate from set_attitude_target message
  343. *
  344. * @return [rad/s] Body pitch rate
  345. */
  346. static inline float mavlink_msg_set_attitude_target_get_body_pitch_rate(const mavlink_message_t* msg)
  347. {
  348. return _MAV_RETURN_float(msg, 24);
  349. }
  350. /**
  351. * @brief Get field body_yaw_rate from set_attitude_target message
  352. *
  353. * @return [rad/s] Body yaw rate
  354. */
  355. static inline float mavlink_msg_set_attitude_target_get_body_yaw_rate(const mavlink_message_t* msg)
  356. {
  357. return _MAV_RETURN_float(msg, 28);
  358. }
  359. /**
  360. * @brief Get field thrust from set_attitude_target message
  361. *
  362. * @return Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)
  363. */
  364. static inline float mavlink_msg_set_attitude_target_get_thrust(const mavlink_message_t* msg)
  365. {
  366. return _MAV_RETURN_float(msg, 32);
  367. }
  368. /**
  369. * @brief Get field thrust_body from set_attitude_target message
  370. *
  371. * @return 3D thrust setpoint in the body NED frame, normalized to -1 .. 1
  372. */
  373. static inline uint16_t mavlink_msg_set_attitude_target_get_thrust_body(const mavlink_message_t* msg, float *thrust_body)
  374. {
  375. return _MAV_RETURN_float_array(msg, thrust_body, 3, 39);
  376. }
  377. /**
  378. * @brief Decode a set_attitude_target message into a struct
  379. *
  380. * @param msg The message to decode
  381. * @param set_attitude_target C-struct to decode the message contents into
  382. */
  383. static inline void mavlink_msg_set_attitude_target_decode(const mavlink_message_t* msg, mavlink_set_attitude_target_t* set_attitude_target)
  384. {
  385. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  386. set_attitude_target->time_boot_ms = mavlink_msg_set_attitude_target_get_time_boot_ms(msg);
  387. mavlink_msg_set_attitude_target_get_q(msg, set_attitude_target->q);
  388. set_attitude_target->body_roll_rate = mavlink_msg_set_attitude_target_get_body_roll_rate(msg);
  389. set_attitude_target->body_pitch_rate = mavlink_msg_set_attitude_target_get_body_pitch_rate(msg);
  390. set_attitude_target->body_yaw_rate = mavlink_msg_set_attitude_target_get_body_yaw_rate(msg);
  391. set_attitude_target->thrust = mavlink_msg_set_attitude_target_get_thrust(msg);
  392. set_attitude_target->target_system = mavlink_msg_set_attitude_target_get_target_system(msg);
  393. set_attitude_target->target_component = mavlink_msg_set_attitude_target_get_target_component(msg);
  394. set_attitude_target->type_mask = mavlink_msg_set_attitude_target_get_type_mask(msg);
  395. mavlink_msg_set_attitude_target_get_thrust_body(msg, set_attitude_target->thrust_body);
  396. #else
  397. uint8_t len = msg->len < MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN? msg->len : MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN;
  398. memset(set_attitude_target, 0, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN);
  399. memcpy(set_attitude_target, _MAV_PAYLOAD(msg), len);
  400. #endif
  401. }