mavlink_msg_gimbal_device_attitude_status.h 30 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480
  1. #pragma once
  2. // MESSAGE GIMBAL_DEVICE_ATTITUDE_STATUS PACKING
  3. #define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS 285
  4. typedef struct __mavlink_gimbal_device_attitude_status_t {
  5. uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/
  6. float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description.*/
  7. float angular_velocity_x; /*< [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown.*/
  8. float angular_velocity_y; /*< [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown.*/
  9. float angular_velocity_z; /*< [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown.*/
  10. uint32_t failure_flags; /*< Failure flags (0 for no failure)*/
  11. uint16_t flags; /*< Current gimbal flags set.*/
  12. uint8_t target_system; /*< System ID*/
  13. uint8_t target_component; /*< Component ID*/
  14. float delta_yaw; /*< [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown.*/
  15. float delta_yaw_velocity; /*< [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown.*/
  16. uint8_t gimbal_device_id; /*< This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.*/
  17. } mavlink_gimbal_device_attitude_status_t;
  18. #define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN 49
  19. #define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN 40
  20. #define MAVLINK_MSG_ID_285_LEN 49
  21. #define MAVLINK_MSG_ID_285_MIN_LEN 40
  22. #define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC 137
  23. #define MAVLINK_MSG_ID_285_CRC 137
  24. #define MAVLINK_MSG_GIMBAL_DEVICE_ATTITUDE_STATUS_FIELD_Q_LEN 4
  25. #if MAVLINK_COMMAND_24BIT
  26. #define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \
  27. 285, \
  28. "GIMBAL_DEVICE_ATTITUDE_STATUS", \
  29. 12, \
  30. { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \
  31. { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \
  32. { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \
  33. { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_gimbal_device_attitude_status_t, flags) }, \
  34. { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_device_attitude_status_t, q) }, \
  35. { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_x) }, \
  36. { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \
  37. { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \
  38. { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \
  39. { "delta_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw) }, \
  40. { "delta_yaw_velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw_velocity) }, \
  41. { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_gimbal_device_attitude_status_t, gimbal_device_id) }, \
  42. } \
  43. }
  44. #else
  45. #define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \
  46. "GIMBAL_DEVICE_ATTITUDE_STATUS", \
  47. 12, \
  48. { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \
  49. { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \
  50. { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \
  51. { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_gimbal_device_attitude_status_t, flags) }, \
  52. { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_device_attitude_status_t, q) }, \
  53. { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_x) }, \
  54. { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \
  55. { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \
  56. { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \
  57. { "delta_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw) }, \
  58. { "delta_yaw_velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw_velocity) }, \
  59. { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_gimbal_device_attitude_status_t, gimbal_device_id) }, \
  60. } \
  61. }
  62. #endif
  63. /**
  64. * @brief Pack a gimbal_device_attitude_status message
  65. * @param system_id ID of this system
  66. * @param component_id ID of this component (e.g. 200 for IMU)
  67. * @param msg The MAVLink message to compress the data into
  68. *
  69. * @param target_system System ID
  70. * @param target_component Component ID
  71. * @param time_boot_ms [ms] Timestamp (time since system boot).
  72. * @param flags Current gimbal flags set.
  73. * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description.
  74. * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown.
  75. * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown.
  76. * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown.
  77. * @param failure_flags Failure flags (0 for no failure)
  78. * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown.
  79. * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown.
  80. * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.
  81. * @return length of the message in bytes (excluding serial stream start sign)
  82. */
  83. static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  84. uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id)
  85. {
  86. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  87. char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN];
  88. _mav_put_uint32_t(buf, 0, time_boot_ms);
  89. _mav_put_float(buf, 20, angular_velocity_x);
  90. _mav_put_float(buf, 24, angular_velocity_y);
  91. _mav_put_float(buf, 28, angular_velocity_z);
  92. _mav_put_uint32_t(buf, 32, failure_flags);
  93. _mav_put_uint16_t(buf, 36, flags);
  94. _mav_put_uint8_t(buf, 38, target_system);
  95. _mav_put_uint8_t(buf, 39, target_component);
  96. _mav_put_float(buf, 40, delta_yaw);
  97. _mav_put_float(buf, 44, delta_yaw_velocity);
  98. _mav_put_uint8_t(buf, 48, gimbal_device_id);
  99. _mav_put_float_array(buf, 4, q, 4);
  100. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN);
  101. #else
  102. mavlink_gimbal_device_attitude_status_t packet;
  103. packet.time_boot_ms = time_boot_ms;
  104. packet.angular_velocity_x = angular_velocity_x;
  105. packet.angular_velocity_y = angular_velocity_y;
  106. packet.angular_velocity_z = angular_velocity_z;
  107. packet.failure_flags = failure_flags;
  108. packet.flags = flags;
  109. packet.target_system = target_system;
  110. packet.target_component = target_component;
  111. packet.delta_yaw = delta_yaw;
  112. packet.delta_yaw_velocity = delta_yaw_velocity;
  113. packet.gimbal_device_id = gimbal_device_id;
  114. mav_array_memcpy(packet.q, q, sizeof(float)*4);
  115. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN);
  116. #endif
  117. msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS;
  118. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  119. }
  120. /**
  121. * @brief Pack a gimbal_device_attitude_status message on a channel
  122. * @param system_id ID of this system
  123. * @param component_id ID of this component (e.g. 200 for IMU)
  124. * @param chan The MAVLink channel this message will be sent over
  125. * @param msg The MAVLink message to compress the data into
  126. * @param target_system System ID
  127. * @param target_component Component ID
  128. * @param time_boot_ms [ms] Timestamp (time since system boot).
  129. * @param flags Current gimbal flags set.
  130. * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description.
  131. * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown.
  132. * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown.
  133. * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown.
  134. * @param failure_flags Failure flags (0 for no failure)
  135. * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown.
  136. * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown.
  137. * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.
  138. * @return length of the message in bytes (excluding serial stream start sign)
  139. */
  140. static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  141. mavlink_message_t* msg,
  142. uint8_t target_system,uint8_t target_component,uint32_t time_boot_ms,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z,uint32_t failure_flags,float delta_yaw,float delta_yaw_velocity,uint8_t gimbal_device_id)
  143. {
  144. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  145. char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN];
  146. _mav_put_uint32_t(buf, 0, time_boot_ms);
  147. _mav_put_float(buf, 20, angular_velocity_x);
  148. _mav_put_float(buf, 24, angular_velocity_y);
  149. _mav_put_float(buf, 28, angular_velocity_z);
  150. _mav_put_uint32_t(buf, 32, failure_flags);
  151. _mav_put_uint16_t(buf, 36, flags);
  152. _mav_put_uint8_t(buf, 38, target_system);
  153. _mav_put_uint8_t(buf, 39, target_component);
  154. _mav_put_float(buf, 40, delta_yaw);
  155. _mav_put_float(buf, 44, delta_yaw_velocity);
  156. _mav_put_uint8_t(buf, 48, gimbal_device_id);
  157. _mav_put_float_array(buf, 4, q, 4);
  158. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN);
  159. #else
  160. mavlink_gimbal_device_attitude_status_t packet;
  161. packet.time_boot_ms = time_boot_ms;
  162. packet.angular_velocity_x = angular_velocity_x;
  163. packet.angular_velocity_y = angular_velocity_y;
  164. packet.angular_velocity_z = angular_velocity_z;
  165. packet.failure_flags = failure_flags;
  166. packet.flags = flags;
  167. packet.target_system = target_system;
  168. packet.target_component = target_component;
  169. packet.delta_yaw = delta_yaw;
  170. packet.delta_yaw_velocity = delta_yaw_velocity;
  171. packet.gimbal_device_id = gimbal_device_id;
  172. mav_array_memcpy(packet.q, q, sizeof(float)*4);
  173. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN);
  174. #endif
  175. msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS;
  176. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  177. }
  178. /**
  179. * @brief Encode a gimbal_device_attitude_status struct
  180. *
  181. * @param system_id ID of this system
  182. * @param component_id ID of this component (e.g. 200 for IMU)
  183. * @param msg The MAVLink message to compress the data into
  184. * @param gimbal_device_attitude_status C-struct to read the message contents from
  185. */
  186. static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status)
  187. {
  188. return mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id);
  189. }
  190. /**
  191. * @brief Encode a gimbal_device_attitude_status struct on a channel
  192. *
  193. * @param system_id ID of this system
  194. * @param component_id ID of this component (e.g. 200 for IMU)
  195. * @param chan The MAVLink channel this message will be sent over
  196. * @param msg The MAVLink message to compress the data into
  197. * @param gimbal_device_attitude_status C-struct to read the message contents from
  198. */
  199. static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status)
  200. {
  201. return mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, chan, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id);
  202. }
  203. /**
  204. * @brief Send a gimbal_device_attitude_status message
  205. * @param chan MAVLink channel to send the message
  206. *
  207. * @param target_system System ID
  208. * @param target_component Component ID
  209. * @param time_boot_ms [ms] Timestamp (time since system boot).
  210. * @param flags Current gimbal flags set.
  211. * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description.
  212. * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown.
  213. * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown.
  214. * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown.
  215. * @param failure_flags Failure flags (0 for no failure)
  216. * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown.
  217. * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown.
  218. * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.
  219. */
  220. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  221. static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id)
  222. {
  223. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  224. char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN];
  225. _mav_put_uint32_t(buf, 0, time_boot_ms);
  226. _mav_put_float(buf, 20, angular_velocity_x);
  227. _mav_put_float(buf, 24, angular_velocity_y);
  228. _mav_put_float(buf, 28, angular_velocity_z);
  229. _mav_put_uint32_t(buf, 32, failure_flags);
  230. _mav_put_uint16_t(buf, 36, flags);
  231. _mav_put_uint8_t(buf, 38, target_system);
  232. _mav_put_uint8_t(buf, 39, target_component);
  233. _mav_put_float(buf, 40, delta_yaw);
  234. _mav_put_float(buf, 44, delta_yaw_velocity);
  235. _mav_put_uint8_t(buf, 48, gimbal_device_id);
  236. _mav_put_float_array(buf, 4, q, 4);
  237. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  238. #else
  239. mavlink_gimbal_device_attitude_status_t packet;
  240. packet.time_boot_ms = time_boot_ms;
  241. packet.angular_velocity_x = angular_velocity_x;
  242. packet.angular_velocity_y = angular_velocity_y;
  243. packet.angular_velocity_z = angular_velocity_z;
  244. packet.failure_flags = failure_flags;
  245. packet.flags = flags;
  246. packet.target_system = target_system;
  247. packet.target_component = target_component;
  248. packet.delta_yaw = delta_yaw;
  249. packet.delta_yaw_velocity = delta_yaw_velocity;
  250. packet.gimbal_device_id = gimbal_device_id;
  251. mav_array_memcpy(packet.q, q, sizeof(float)*4);
  252. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  253. #endif
  254. }
  255. /**
  256. * @brief Send a gimbal_device_attitude_status message
  257. * @param chan MAVLink channel to send the message
  258. * @param struct The MAVLink struct to serialize
  259. */
  260. static inline void mavlink_msg_gimbal_device_attitude_status_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status)
  261. {
  262. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  263. mavlink_msg_gimbal_device_attitude_status_send(chan, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id);
  264. #else
  265. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)gimbal_device_attitude_status, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  266. #endif
  267. }
  268. #if MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  269. /*
  270. This variant of _send() can be used to save stack space by re-using
  271. memory from the receive buffer. The caller provides a
  272. mavlink_message_t which is the size of a full mavlink message. This
  273. is usually the receive buffer for the channel, and allows a reply to an
  274. incoming message with minimum stack space usage.
  275. */
  276. static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id)
  277. {
  278. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  279. char *buf = (char *)msgbuf;
  280. _mav_put_uint32_t(buf, 0, time_boot_ms);
  281. _mav_put_float(buf, 20, angular_velocity_x);
  282. _mav_put_float(buf, 24, angular_velocity_y);
  283. _mav_put_float(buf, 28, angular_velocity_z);
  284. _mav_put_uint32_t(buf, 32, failure_flags);
  285. _mav_put_uint16_t(buf, 36, flags);
  286. _mav_put_uint8_t(buf, 38, target_system);
  287. _mav_put_uint8_t(buf, 39, target_component);
  288. _mav_put_float(buf, 40, delta_yaw);
  289. _mav_put_float(buf, 44, delta_yaw_velocity);
  290. _mav_put_uint8_t(buf, 48, gimbal_device_id);
  291. _mav_put_float_array(buf, 4, q, 4);
  292. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  293. #else
  294. mavlink_gimbal_device_attitude_status_t *packet = (mavlink_gimbal_device_attitude_status_t *)msgbuf;
  295. packet->time_boot_ms = time_boot_ms;
  296. packet->angular_velocity_x = angular_velocity_x;
  297. packet->angular_velocity_y = angular_velocity_y;
  298. packet->angular_velocity_z = angular_velocity_z;
  299. packet->failure_flags = failure_flags;
  300. packet->flags = flags;
  301. packet->target_system = target_system;
  302. packet->target_component = target_component;
  303. packet->delta_yaw = delta_yaw;
  304. packet->delta_yaw_velocity = delta_yaw_velocity;
  305. packet->gimbal_device_id = gimbal_device_id;
  306. mav_array_memcpy(packet->q, q, sizeof(float)*4);
  307. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC);
  308. #endif
  309. }
  310. #endif
  311. #endif
  312. // MESSAGE GIMBAL_DEVICE_ATTITUDE_STATUS UNPACKING
  313. /**
  314. * @brief Get field target_system from gimbal_device_attitude_status message
  315. *
  316. * @return System ID
  317. */
  318. static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_target_system(const mavlink_message_t* msg)
  319. {
  320. return _MAV_RETURN_uint8_t(msg, 38);
  321. }
  322. /**
  323. * @brief Get field target_component from gimbal_device_attitude_status message
  324. *
  325. * @return Component ID
  326. */
  327. static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_target_component(const mavlink_message_t* msg)
  328. {
  329. return _MAV_RETURN_uint8_t(msg, 39);
  330. }
  331. /**
  332. * @brief Get field time_boot_ms from gimbal_device_attitude_status message
  333. *
  334. * @return [ms] Timestamp (time since system boot).
  335. */
  336. static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_time_boot_ms(const mavlink_message_t* msg)
  337. {
  338. return _MAV_RETURN_uint32_t(msg, 0);
  339. }
  340. /**
  341. * @brief Get field flags from gimbal_device_attitude_status message
  342. *
  343. * @return Current gimbal flags set.
  344. */
  345. static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_flags(const mavlink_message_t* msg)
  346. {
  347. return _MAV_RETURN_uint16_t(msg, 36);
  348. }
  349. /**
  350. * @brief Get field q from gimbal_device_attitude_status message
  351. *
  352. * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description.
  353. */
  354. static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_q(const mavlink_message_t* msg, float *q)
  355. {
  356. return _MAV_RETURN_float_array(msg, q, 4, 4);
  357. }
  358. /**
  359. * @brief Get field angular_velocity_x from gimbal_device_attitude_status message
  360. *
  361. * @return [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown.
  362. */
  363. static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(const mavlink_message_t* msg)
  364. {
  365. return _MAV_RETURN_float(msg, 20);
  366. }
  367. /**
  368. * @brief Get field angular_velocity_y from gimbal_device_attitude_status message
  369. *
  370. * @return [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown.
  371. */
  372. static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(const mavlink_message_t* msg)
  373. {
  374. return _MAV_RETURN_float(msg, 24);
  375. }
  376. /**
  377. * @brief Get field angular_velocity_z from gimbal_device_attitude_status message
  378. *
  379. * @return [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown.
  380. */
  381. static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(const mavlink_message_t* msg)
  382. {
  383. return _MAV_RETURN_float(msg, 28);
  384. }
  385. /**
  386. * @brief Get field failure_flags from gimbal_device_attitude_status message
  387. *
  388. * @return Failure flags (0 for no failure)
  389. */
  390. static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_failure_flags(const mavlink_message_t* msg)
  391. {
  392. return _MAV_RETURN_uint32_t(msg, 32);
  393. }
  394. /**
  395. * @brief Get field delta_yaw from gimbal_device_attitude_status message
  396. *
  397. * @return [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown.
  398. */
  399. static inline float mavlink_msg_gimbal_device_attitude_status_get_delta_yaw(const mavlink_message_t* msg)
  400. {
  401. return _MAV_RETURN_float(msg, 40);
  402. }
  403. /**
  404. * @brief Get field delta_yaw_velocity from gimbal_device_attitude_status message
  405. *
  406. * @return [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown.
  407. */
  408. static inline float mavlink_msg_gimbal_device_attitude_status_get_delta_yaw_velocity(const mavlink_message_t* msg)
  409. {
  410. return _MAV_RETURN_float(msg, 44);
  411. }
  412. /**
  413. * @brief Get field gimbal_device_id from gimbal_device_attitude_status message
  414. *
  415. * @return This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.
  416. */
  417. static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_gimbal_device_id(const mavlink_message_t* msg)
  418. {
  419. return _MAV_RETURN_uint8_t(msg, 48);
  420. }
  421. /**
  422. * @brief Decode a gimbal_device_attitude_status message into a struct
  423. *
  424. * @param msg The message to decode
  425. * @param gimbal_device_attitude_status C-struct to decode the message contents into
  426. */
  427. static inline void mavlink_msg_gimbal_device_attitude_status_decode(const mavlink_message_t* msg, mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status)
  428. {
  429. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  430. gimbal_device_attitude_status->time_boot_ms = mavlink_msg_gimbal_device_attitude_status_get_time_boot_ms(msg);
  431. mavlink_msg_gimbal_device_attitude_status_get_q(msg, gimbal_device_attitude_status->q);
  432. gimbal_device_attitude_status->angular_velocity_x = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(msg);
  433. gimbal_device_attitude_status->angular_velocity_y = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(msg);
  434. gimbal_device_attitude_status->angular_velocity_z = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(msg);
  435. gimbal_device_attitude_status->failure_flags = mavlink_msg_gimbal_device_attitude_status_get_failure_flags(msg);
  436. gimbal_device_attitude_status->flags = mavlink_msg_gimbal_device_attitude_status_get_flags(msg);
  437. gimbal_device_attitude_status->target_system = mavlink_msg_gimbal_device_attitude_status_get_target_system(msg);
  438. gimbal_device_attitude_status->target_component = mavlink_msg_gimbal_device_attitude_status_get_target_component(msg);
  439. gimbal_device_attitude_status->delta_yaw = mavlink_msg_gimbal_device_attitude_status_get_delta_yaw(msg);
  440. gimbal_device_attitude_status->delta_yaw_velocity = mavlink_msg_gimbal_device_attitude_status_get_delta_yaw_velocity(msg);
  441. gimbal_device_attitude_status->gimbal_device_id = mavlink_msg_gimbal_device_attitude_status_get_gimbal_device_id(msg);
  442. #else
  443. uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN;
  444. memset(gimbal_device_attitude_status, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN);
  445. memcpy(gimbal_device_attitude_status, _MAV_PAYLOAD(msg), len);
  446. #endif
  447. }