mavlink_msg_servo_output_raw.h 30 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638
  1. #pragma once
  2. // MESSAGE SERVO_OUTPUT_RAW PACKING
  3. #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36
  4. MAVPACKED(
  5. typedef struct __mavlink_servo_output_raw_t {
  6. uint32_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/
  7. uint16_t servo1_raw; /*< [us] Servo output 1 value*/
  8. uint16_t servo2_raw; /*< [us] Servo output 2 value*/
  9. uint16_t servo3_raw; /*< [us] Servo output 3 value*/
  10. uint16_t servo4_raw; /*< [us] Servo output 4 value*/
  11. uint16_t servo5_raw; /*< [us] Servo output 5 value*/
  12. uint16_t servo6_raw; /*< [us] Servo output 6 value*/
  13. uint16_t servo7_raw; /*< [us] Servo output 7 value*/
  14. uint16_t servo8_raw; /*< [us] Servo output 8 value*/
  15. uint8_t port; /*< Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.*/
  16. uint16_t servo9_raw; /*< [us] Servo output 9 value*/
  17. uint16_t servo10_raw; /*< [us] Servo output 10 value*/
  18. uint16_t servo11_raw; /*< [us] Servo output 11 value*/
  19. uint16_t servo12_raw; /*< [us] Servo output 12 value*/
  20. uint16_t servo13_raw; /*< [us] Servo output 13 value*/
  21. uint16_t servo14_raw; /*< [us] Servo output 14 value*/
  22. uint16_t servo15_raw; /*< [us] Servo output 15 value*/
  23. uint16_t servo16_raw; /*< [us] Servo output 16 value*/
  24. }) mavlink_servo_output_raw_t;
  25. #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 37
  26. #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN 21
  27. #define MAVLINK_MSG_ID_36_LEN 37
  28. #define MAVLINK_MSG_ID_36_MIN_LEN 21
  29. #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC 222
  30. #define MAVLINK_MSG_ID_36_CRC 222
  31. #if MAVLINK_COMMAND_24BIT
  32. #define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \
  33. 36, \
  34. "SERVO_OUTPUT_RAW", \
  35. 18, \
  36. { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \
  37. { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \
  38. { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \
  39. { "servo2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \
  40. { "servo3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \
  41. { "servo4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \
  42. { "servo5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \
  43. { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \
  44. { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \
  45. { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \
  46. { "servo9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_servo_output_raw_t, servo9_raw) }, \
  47. { "servo10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 23, offsetof(mavlink_servo_output_raw_t, servo10_raw) }, \
  48. { "servo11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 25, offsetof(mavlink_servo_output_raw_t, servo11_raw) }, \
  49. { "servo12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 27, offsetof(mavlink_servo_output_raw_t, servo12_raw) }, \
  50. { "servo13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 29, offsetof(mavlink_servo_output_raw_t, servo13_raw) }, \
  51. { "servo14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 31, offsetof(mavlink_servo_output_raw_t, servo14_raw) }, \
  52. { "servo15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 33, offsetof(mavlink_servo_output_raw_t, servo15_raw) }, \
  53. { "servo16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_servo_output_raw_t, servo16_raw) }, \
  54. } \
  55. }
  56. #else
  57. #define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \
  58. "SERVO_OUTPUT_RAW", \
  59. 18, \
  60. { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \
  61. { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \
  62. { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \
  63. { "servo2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \
  64. { "servo3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \
  65. { "servo4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \
  66. { "servo5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \
  67. { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \
  68. { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \
  69. { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \
  70. { "servo9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_servo_output_raw_t, servo9_raw) }, \
  71. { "servo10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 23, offsetof(mavlink_servo_output_raw_t, servo10_raw) }, \
  72. { "servo11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 25, offsetof(mavlink_servo_output_raw_t, servo11_raw) }, \
  73. { "servo12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 27, offsetof(mavlink_servo_output_raw_t, servo12_raw) }, \
  74. { "servo13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 29, offsetof(mavlink_servo_output_raw_t, servo13_raw) }, \
  75. { "servo14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 31, offsetof(mavlink_servo_output_raw_t, servo14_raw) }, \
  76. { "servo15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 33, offsetof(mavlink_servo_output_raw_t, servo15_raw) }, \
  77. { "servo16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_servo_output_raw_t, servo16_raw) }, \
  78. } \
  79. }
  80. #endif
  81. /**
  82. * @brief Pack a servo_output_raw message
  83. * @param system_id ID of this system
  84. * @param component_id ID of this component (e.g. 200 for IMU)
  85. * @param msg The MAVLink message to compress the data into
  86. *
  87. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  88. * @param port Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.
  89. * @param servo1_raw [us] Servo output 1 value
  90. * @param servo2_raw [us] Servo output 2 value
  91. * @param servo3_raw [us] Servo output 3 value
  92. * @param servo4_raw [us] Servo output 4 value
  93. * @param servo5_raw [us] Servo output 5 value
  94. * @param servo6_raw [us] Servo output 6 value
  95. * @param servo7_raw [us] Servo output 7 value
  96. * @param servo8_raw [us] Servo output 8 value
  97. * @param servo9_raw [us] Servo output 9 value
  98. * @param servo10_raw [us] Servo output 10 value
  99. * @param servo11_raw [us] Servo output 11 value
  100. * @param servo12_raw [us] Servo output 12 value
  101. * @param servo13_raw [us] Servo output 13 value
  102. * @param servo14_raw [us] Servo output 14 value
  103. * @param servo15_raw [us] Servo output 15 value
  104. * @param servo16_raw [us] Servo output 16 value
  105. * @return length of the message in bytes (excluding serial stream start sign)
  106. */
  107. static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  108. uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw)
  109. {
  110. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  111. char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN];
  112. _mav_put_uint32_t(buf, 0, time_usec);
  113. _mav_put_uint16_t(buf, 4, servo1_raw);
  114. _mav_put_uint16_t(buf, 6, servo2_raw);
  115. _mav_put_uint16_t(buf, 8, servo3_raw);
  116. _mav_put_uint16_t(buf, 10, servo4_raw);
  117. _mav_put_uint16_t(buf, 12, servo5_raw);
  118. _mav_put_uint16_t(buf, 14, servo6_raw);
  119. _mav_put_uint16_t(buf, 16, servo7_raw);
  120. _mav_put_uint16_t(buf, 18, servo8_raw);
  121. _mav_put_uint8_t(buf, 20, port);
  122. _mav_put_uint16_t(buf, 21, servo9_raw);
  123. _mav_put_uint16_t(buf, 23, servo10_raw);
  124. _mav_put_uint16_t(buf, 25, servo11_raw);
  125. _mav_put_uint16_t(buf, 27, servo12_raw);
  126. _mav_put_uint16_t(buf, 29, servo13_raw);
  127. _mav_put_uint16_t(buf, 31, servo14_raw);
  128. _mav_put_uint16_t(buf, 33, servo15_raw);
  129. _mav_put_uint16_t(buf, 35, servo16_raw);
  130. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
  131. #else
  132. mavlink_servo_output_raw_t packet;
  133. packet.time_usec = time_usec;
  134. packet.servo1_raw = servo1_raw;
  135. packet.servo2_raw = servo2_raw;
  136. packet.servo3_raw = servo3_raw;
  137. packet.servo4_raw = servo4_raw;
  138. packet.servo5_raw = servo5_raw;
  139. packet.servo6_raw = servo6_raw;
  140. packet.servo7_raw = servo7_raw;
  141. packet.servo8_raw = servo8_raw;
  142. packet.port = port;
  143. packet.servo9_raw = servo9_raw;
  144. packet.servo10_raw = servo10_raw;
  145. packet.servo11_raw = servo11_raw;
  146. packet.servo12_raw = servo12_raw;
  147. packet.servo13_raw = servo13_raw;
  148. packet.servo14_raw = servo14_raw;
  149. packet.servo15_raw = servo15_raw;
  150. packet.servo16_raw = servo16_raw;
  151. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
  152. #endif
  153. msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
  154. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  155. }
  156. /**
  157. * @brief Pack a servo_output_raw message on a channel
  158. * @param system_id ID of this system
  159. * @param component_id ID of this component (e.g. 200 for IMU)
  160. * @param chan The MAVLink channel this message will be sent over
  161. * @param msg The MAVLink message to compress the data into
  162. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  163. * @param port Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.
  164. * @param servo1_raw [us] Servo output 1 value
  165. * @param servo2_raw [us] Servo output 2 value
  166. * @param servo3_raw [us] Servo output 3 value
  167. * @param servo4_raw [us] Servo output 4 value
  168. * @param servo5_raw [us] Servo output 5 value
  169. * @param servo6_raw [us] Servo output 6 value
  170. * @param servo7_raw [us] Servo output 7 value
  171. * @param servo8_raw [us] Servo output 8 value
  172. * @param servo9_raw [us] Servo output 9 value
  173. * @param servo10_raw [us] Servo output 10 value
  174. * @param servo11_raw [us] Servo output 11 value
  175. * @param servo12_raw [us] Servo output 12 value
  176. * @param servo13_raw [us] Servo output 13 value
  177. * @param servo14_raw [us] Servo output 14 value
  178. * @param servo15_raw [us] Servo output 15 value
  179. * @param servo16_raw [us] Servo output 16 value
  180. * @return length of the message in bytes (excluding serial stream start sign)
  181. */
  182. static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  183. mavlink_message_t* msg,
  184. uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw,uint16_t servo9_raw,uint16_t servo10_raw,uint16_t servo11_raw,uint16_t servo12_raw,uint16_t servo13_raw,uint16_t servo14_raw,uint16_t servo15_raw,uint16_t servo16_raw)
  185. {
  186. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  187. char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN];
  188. _mav_put_uint32_t(buf, 0, time_usec);
  189. _mav_put_uint16_t(buf, 4, servo1_raw);
  190. _mav_put_uint16_t(buf, 6, servo2_raw);
  191. _mav_put_uint16_t(buf, 8, servo3_raw);
  192. _mav_put_uint16_t(buf, 10, servo4_raw);
  193. _mav_put_uint16_t(buf, 12, servo5_raw);
  194. _mav_put_uint16_t(buf, 14, servo6_raw);
  195. _mav_put_uint16_t(buf, 16, servo7_raw);
  196. _mav_put_uint16_t(buf, 18, servo8_raw);
  197. _mav_put_uint8_t(buf, 20, port);
  198. _mav_put_uint16_t(buf, 21, servo9_raw);
  199. _mav_put_uint16_t(buf, 23, servo10_raw);
  200. _mav_put_uint16_t(buf, 25, servo11_raw);
  201. _mav_put_uint16_t(buf, 27, servo12_raw);
  202. _mav_put_uint16_t(buf, 29, servo13_raw);
  203. _mav_put_uint16_t(buf, 31, servo14_raw);
  204. _mav_put_uint16_t(buf, 33, servo15_raw);
  205. _mav_put_uint16_t(buf, 35, servo16_raw);
  206. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
  207. #else
  208. mavlink_servo_output_raw_t packet;
  209. packet.time_usec = time_usec;
  210. packet.servo1_raw = servo1_raw;
  211. packet.servo2_raw = servo2_raw;
  212. packet.servo3_raw = servo3_raw;
  213. packet.servo4_raw = servo4_raw;
  214. packet.servo5_raw = servo5_raw;
  215. packet.servo6_raw = servo6_raw;
  216. packet.servo7_raw = servo7_raw;
  217. packet.servo8_raw = servo8_raw;
  218. packet.port = port;
  219. packet.servo9_raw = servo9_raw;
  220. packet.servo10_raw = servo10_raw;
  221. packet.servo11_raw = servo11_raw;
  222. packet.servo12_raw = servo12_raw;
  223. packet.servo13_raw = servo13_raw;
  224. packet.servo14_raw = servo14_raw;
  225. packet.servo15_raw = servo15_raw;
  226. packet.servo16_raw = servo16_raw;
  227. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
  228. #endif
  229. msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
  230. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  231. }
  232. /**
  233. * @brief Encode a servo_output_raw struct
  234. *
  235. * @param system_id ID of this system
  236. * @param component_id ID of this component (e.g. 200 for IMU)
  237. * @param msg The MAVLink message to compress the data into
  238. * @param servo_output_raw C-struct to read the message contents from
  239. */
  240. static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw)
  241. {
  242. return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw);
  243. }
  244. /**
  245. * @brief Encode a servo_output_raw struct on a channel
  246. *
  247. * @param system_id ID of this system
  248. * @param component_id ID of this component (e.g. 200 for IMU)
  249. * @param chan The MAVLink channel this message will be sent over
  250. * @param msg The MAVLink message to compress the data into
  251. * @param servo_output_raw C-struct to read the message contents from
  252. */
  253. static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw)
  254. {
  255. return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw);
  256. }
  257. /**
  258. * @brief Send a servo_output_raw message
  259. * @param chan MAVLink channel to send the message
  260. *
  261. * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  262. * @param port Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.
  263. * @param servo1_raw [us] Servo output 1 value
  264. * @param servo2_raw [us] Servo output 2 value
  265. * @param servo3_raw [us] Servo output 3 value
  266. * @param servo4_raw [us] Servo output 4 value
  267. * @param servo5_raw [us] Servo output 5 value
  268. * @param servo6_raw [us] Servo output 6 value
  269. * @param servo7_raw [us] Servo output 7 value
  270. * @param servo8_raw [us] Servo output 8 value
  271. * @param servo9_raw [us] Servo output 9 value
  272. * @param servo10_raw [us] Servo output 10 value
  273. * @param servo11_raw [us] Servo output 11 value
  274. * @param servo12_raw [us] Servo output 12 value
  275. * @param servo13_raw [us] Servo output 13 value
  276. * @param servo14_raw [us] Servo output 14 value
  277. * @param servo15_raw [us] Servo output 15 value
  278. * @param servo16_raw [us] Servo output 16 value
  279. */
  280. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  281. static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw)
  282. {
  283. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  284. char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN];
  285. _mav_put_uint32_t(buf, 0, time_usec);
  286. _mav_put_uint16_t(buf, 4, servo1_raw);
  287. _mav_put_uint16_t(buf, 6, servo2_raw);
  288. _mav_put_uint16_t(buf, 8, servo3_raw);
  289. _mav_put_uint16_t(buf, 10, servo4_raw);
  290. _mav_put_uint16_t(buf, 12, servo5_raw);
  291. _mav_put_uint16_t(buf, 14, servo6_raw);
  292. _mav_put_uint16_t(buf, 16, servo7_raw);
  293. _mav_put_uint16_t(buf, 18, servo8_raw);
  294. _mav_put_uint8_t(buf, 20, port);
  295. _mav_put_uint16_t(buf, 21, servo9_raw);
  296. _mav_put_uint16_t(buf, 23, servo10_raw);
  297. _mav_put_uint16_t(buf, 25, servo11_raw);
  298. _mav_put_uint16_t(buf, 27, servo12_raw);
  299. _mav_put_uint16_t(buf, 29, servo13_raw);
  300. _mav_put_uint16_t(buf, 31, servo14_raw);
  301. _mav_put_uint16_t(buf, 33, servo15_raw);
  302. _mav_put_uint16_t(buf, 35, servo16_raw);
  303. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  304. #else
  305. mavlink_servo_output_raw_t packet;
  306. packet.time_usec = time_usec;
  307. packet.servo1_raw = servo1_raw;
  308. packet.servo2_raw = servo2_raw;
  309. packet.servo3_raw = servo3_raw;
  310. packet.servo4_raw = servo4_raw;
  311. packet.servo5_raw = servo5_raw;
  312. packet.servo6_raw = servo6_raw;
  313. packet.servo7_raw = servo7_raw;
  314. packet.servo8_raw = servo8_raw;
  315. packet.port = port;
  316. packet.servo9_raw = servo9_raw;
  317. packet.servo10_raw = servo10_raw;
  318. packet.servo11_raw = servo11_raw;
  319. packet.servo12_raw = servo12_raw;
  320. packet.servo13_raw = servo13_raw;
  321. packet.servo14_raw = servo14_raw;
  322. packet.servo15_raw = servo15_raw;
  323. packet.servo16_raw = servo16_raw;
  324. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  325. #endif
  326. }
  327. /**
  328. * @brief Send a servo_output_raw message
  329. * @param chan MAVLink channel to send the message
  330. * @param struct The MAVLink struct to serialize
  331. */
  332. static inline void mavlink_msg_servo_output_raw_send_struct(mavlink_channel_t chan, const mavlink_servo_output_raw_t* servo_output_raw)
  333. {
  334. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  335. mavlink_msg_servo_output_raw_send(chan, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw);
  336. #else
  337. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)servo_output_raw, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  338. #endif
  339. }
  340. #if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  341. /*
  342. This variant of _send() can be used to save stack space by re-using
  343. memory from the receive buffer. The caller provides a
  344. mavlink_message_t which is the size of a full mavlink message. This
  345. is usually the receive buffer for the channel, and allows a reply to an
  346. incoming message with minimum stack space usage.
  347. */
  348. static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw)
  349. {
  350. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  351. char *buf = (char *)msgbuf;
  352. _mav_put_uint32_t(buf, 0, time_usec);
  353. _mav_put_uint16_t(buf, 4, servo1_raw);
  354. _mav_put_uint16_t(buf, 6, servo2_raw);
  355. _mav_put_uint16_t(buf, 8, servo3_raw);
  356. _mav_put_uint16_t(buf, 10, servo4_raw);
  357. _mav_put_uint16_t(buf, 12, servo5_raw);
  358. _mav_put_uint16_t(buf, 14, servo6_raw);
  359. _mav_put_uint16_t(buf, 16, servo7_raw);
  360. _mav_put_uint16_t(buf, 18, servo8_raw);
  361. _mav_put_uint8_t(buf, 20, port);
  362. _mav_put_uint16_t(buf, 21, servo9_raw);
  363. _mav_put_uint16_t(buf, 23, servo10_raw);
  364. _mav_put_uint16_t(buf, 25, servo11_raw);
  365. _mav_put_uint16_t(buf, 27, servo12_raw);
  366. _mav_put_uint16_t(buf, 29, servo13_raw);
  367. _mav_put_uint16_t(buf, 31, servo14_raw);
  368. _mav_put_uint16_t(buf, 33, servo15_raw);
  369. _mav_put_uint16_t(buf, 35, servo16_raw);
  370. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  371. #else
  372. mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf;
  373. packet->time_usec = time_usec;
  374. packet->servo1_raw = servo1_raw;
  375. packet->servo2_raw = servo2_raw;
  376. packet->servo3_raw = servo3_raw;
  377. packet->servo4_raw = servo4_raw;
  378. packet->servo5_raw = servo5_raw;
  379. packet->servo6_raw = servo6_raw;
  380. packet->servo7_raw = servo7_raw;
  381. packet->servo8_raw = servo8_raw;
  382. packet->port = port;
  383. packet->servo9_raw = servo9_raw;
  384. packet->servo10_raw = servo10_raw;
  385. packet->servo11_raw = servo11_raw;
  386. packet->servo12_raw = servo12_raw;
  387. packet->servo13_raw = servo13_raw;
  388. packet->servo14_raw = servo14_raw;
  389. packet->servo15_raw = servo15_raw;
  390. packet->servo16_raw = servo16_raw;
  391. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
  392. #endif
  393. }
  394. #endif
  395. #endif
  396. // MESSAGE SERVO_OUTPUT_RAW UNPACKING
  397. /**
  398. * @brief Get field time_usec from servo_output_raw message
  399. *
  400. * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
  401. */
  402. static inline uint32_t mavlink_msg_servo_output_raw_get_time_usec(const mavlink_message_t* msg)
  403. {
  404. return _MAV_RETURN_uint32_t(msg, 0);
  405. }
  406. /**
  407. * @brief Get field port from servo_output_raw message
  408. *
  409. * @return Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.
  410. */
  411. static inline uint8_t mavlink_msg_servo_output_raw_get_port(const mavlink_message_t* msg)
  412. {
  413. return _MAV_RETURN_uint8_t(msg, 20);
  414. }
  415. /**
  416. * @brief Get field servo1_raw from servo_output_raw message
  417. *
  418. * @return [us] Servo output 1 value
  419. */
  420. static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg)
  421. {
  422. return _MAV_RETURN_uint16_t(msg, 4);
  423. }
  424. /**
  425. * @brief Get field servo2_raw from servo_output_raw message
  426. *
  427. * @return [us] Servo output 2 value
  428. */
  429. static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg)
  430. {
  431. return _MAV_RETURN_uint16_t(msg, 6);
  432. }
  433. /**
  434. * @brief Get field servo3_raw from servo_output_raw message
  435. *
  436. * @return [us] Servo output 3 value
  437. */
  438. static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg)
  439. {
  440. return _MAV_RETURN_uint16_t(msg, 8);
  441. }
  442. /**
  443. * @brief Get field servo4_raw from servo_output_raw message
  444. *
  445. * @return [us] Servo output 4 value
  446. */
  447. static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg)
  448. {
  449. return _MAV_RETURN_uint16_t(msg, 10);
  450. }
  451. /**
  452. * @brief Get field servo5_raw from servo_output_raw message
  453. *
  454. * @return [us] Servo output 5 value
  455. */
  456. static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg)
  457. {
  458. return _MAV_RETURN_uint16_t(msg, 12);
  459. }
  460. /**
  461. * @brief Get field servo6_raw from servo_output_raw message
  462. *
  463. * @return [us] Servo output 6 value
  464. */
  465. static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg)
  466. {
  467. return _MAV_RETURN_uint16_t(msg, 14);
  468. }
  469. /**
  470. * @brief Get field servo7_raw from servo_output_raw message
  471. *
  472. * @return [us] Servo output 7 value
  473. */
  474. static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg)
  475. {
  476. return _MAV_RETURN_uint16_t(msg, 16);
  477. }
  478. /**
  479. * @brief Get field servo8_raw from servo_output_raw message
  480. *
  481. * @return [us] Servo output 8 value
  482. */
  483. static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg)
  484. {
  485. return _MAV_RETURN_uint16_t(msg, 18);
  486. }
  487. /**
  488. * @brief Get field servo9_raw from servo_output_raw message
  489. *
  490. * @return [us] Servo output 9 value
  491. */
  492. static inline uint16_t mavlink_msg_servo_output_raw_get_servo9_raw(const mavlink_message_t* msg)
  493. {
  494. return _MAV_RETURN_uint16_t(msg, 21);
  495. }
  496. /**
  497. * @brief Get field servo10_raw from servo_output_raw message
  498. *
  499. * @return [us] Servo output 10 value
  500. */
  501. static inline uint16_t mavlink_msg_servo_output_raw_get_servo10_raw(const mavlink_message_t* msg)
  502. {
  503. return _MAV_RETURN_uint16_t(msg, 23);
  504. }
  505. /**
  506. * @brief Get field servo11_raw from servo_output_raw message
  507. *
  508. * @return [us] Servo output 11 value
  509. */
  510. static inline uint16_t mavlink_msg_servo_output_raw_get_servo11_raw(const mavlink_message_t* msg)
  511. {
  512. return _MAV_RETURN_uint16_t(msg, 25);
  513. }
  514. /**
  515. * @brief Get field servo12_raw from servo_output_raw message
  516. *
  517. * @return [us] Servo output 12 value
  518. */
  519. static inline uint16_t mavlink_msg_servo_output_raw_get_servo12_raw(const mavlink_message_t* msg)
  520. {
  521. return _MAV_RETURN_uint16_t(msg, 27);
  522. }
  523. /**
  524. * @brief Get field servo13_raw from servo_output_raw message
  525. *
  526. * @return [us] Servo output 13 value
  527. */
  528. static inline uint16_t mavlink_msg_servo_output_raw_get_servo13_raw(const mavlink_message_t* msg)
  529. {
  530. return _MAV_RETURN_uint16_t(msg, 29);
  531. }
  532. /**
  533. * @brief Get field servo14_raw from servo_output_raw message
  534. *
  535. * @return [us] Servo output 14 value
  536. */
  537. static inline uint16_t mavlink_msg_servo_output_raw_get_servo14_raw(const mavlink_message_t* msg)
  538. {
  539. return _MAV_RETURN_uint16_t(msg, 31);
  540. }
  541. /**
  542. * @brief Get field servo15_raw from servo_output_raw message
  543. *
  544. * @return [us] Servo output 15 value
  545. */
  546. static inline uint16_t mavlink_msg_servo_output_raw_get_servo15_raw(const mavlink_message_t* msg)
  547. {
  548. return _MAV_RETURN_uint16_t(msg, 33);
  549. }
  550. /**
  551. * @brief Get field servo16_raw from servo_output_raw message
  552. *
  553. * @return [us] Servo output 16 value
  554. */
  555. static inline uint16_t mavlink_msg_servo_output_raw_get_servo16_raw(const mavlink_message_t* msg)
  556. {
  557. return _MAV_RETURN_uint16_t(msg, 35);
  558. }
  559. /**
  560. * @brief Decode a servo_output_raw message into a struct
  561. *
  562. * @param msg The message to decode
  563. * @param servo_output_raw C-struct to decode the message contents into
  564. */
  565. static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw)
  566. {
  567. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  568. servo_output_raw->time_usec = mavlink_msg_servo_output_raw_get_time_usec(msg);
  569. servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg);
  570. servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg);
  571. servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg);
  572. servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg);
  573. servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg);
  574. servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg);
  575. servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg);
  576. servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg);
  577. servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg);
  578. servo_output_raw->servo9_raw = mavlink_msg_servo_output_raw_get_servo9_raw(msg);
  579. servo_output_raw->servo10_raw = mavlink_msg_servo_output_raw_get_servo10_raw(msg);
  580. servo_output_raw->servo11_raw = mavlink_msg_servo_output_raw_get_servo11_raw(msg);
  581. servo_output_raw->servo12_raw = mavlink_msg_servo_output_raw_get_servo12_raw(msg);
  582. servo_output_raw->servo13_raw = mavlink_msg_servo_output_raw_get_servo13_raw(msg);
  583. servo_output_raw->servo14_raw = mavlink_msg_servo_output_raw_get_servo14_raw(msg);
  584. servo_output_raw->servo15_raw = mavlink_msg_servo_output_raw_get_servo15_raw(msg);
  585. servo_output_raw->servo16_raw = mavlink_msg_servo_output_raw_get_servo16_raw(msg);
  586. #else
  587. uint8_t len = msg->len < MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN? msg->len : MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN;
  588. memset(servo_output_raw, 0, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
  589. memcpy(servo_output_raw, _MAV_PAYLOAD(msg), len);
  590. #endif
  591. }