mavlink_msg_sys_status.h 36 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588
  1. #pragma once
  2. // MESSAGE SYS_STATUS PACKING
  3. #define MAVLINK_MSG_ID_SYS_STATUS 1
  4. MAVPACKED(
  5. typedef struct __mavlink_sys_status_t {
  6. uint32_t onboard_control_sensors_present; /*< Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.*/
  7. uint32_t onboard_control_sensors_enabled; /*< Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.*/
  8. uint32_t onboard_control_sensors_health; /*< Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.*/
  9. uint16_t load; /*< [d%] Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000*/
  10. uint16_t voltage_battery; /*< [mV] Battery voltage, UINT16_MAX: Voltage not sent by autopilot*/
  11. int16_t current_battery; /*< [cA] Battery current, -1: Current not sent by autopilot*/
  12. uint16_t drop_rate_comm; /*< [c%] Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)*/
  13. uint16_t errors_comm; /*< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)*/
  14. uint16_t errors_count1; /*< Autopilot-specific errors*/
  15. uint16_t errors_count2; /*< Autopilot-specific errors*/
  16. uint16_t errors_count3; /*< Autopilot-specific errors*/
  17. uint16_t errors_count4; /*< Autopilot-specific errors*/
  18. int8_t battery_remaining; /*< [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot*/
  19. uint32_t onboard_control_sensors_present_extended; /*< Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.*/
  20. uint32_t onboard_control_sensors_enabled_extended; /*< Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.*/
  21. uint32_t onboard_control_sensors_health_extended; /*< Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.*/
  22. }) mavlink_sys_status_t;
  23. #define MAVLINK_MSG_ID_SYS_STATUS_LEN 43
  24. #define MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN 31
  25. #define MAVLINK_MSG_ID_1_LEN 43
  26. #define MAVLINK_MSG_ID_1_MIN_LEN 31
  27. #define MAVLINK_MSG_ID_SYS_STATUS_CRC 124
  28. #define MAVLINK_MSG_ID_1_CRC 124
  29. #if MAVLINK_COMMAND_24BIT
  30. #define MAVLINK_MESSAGE_INFO_SYS_STATUS { \
  31. 1, \
  32. "SYS_STATUS", \
  33. 16, \
  34. { { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \
  35. { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \
  36. { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \
  37. { "load", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_sys_status_t, load) }, \
  38. { "voltage_battery", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, voltage_battery) }, \
  39. { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_sys_status_t, current_battery) }, \
  40. { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_sys_status_t, battery_remaining) }, \
  41. { "drop_rate_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, drop_rate_comm) }, \
  42. { "errors_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_comm) }, \
  43. { "errors_count1", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_count1) }, \
  44. { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \
  45. { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \
  46. { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \
  47. { "onboard_control_sensors_present_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 31, offsetof(mavlink_sys_status_t, onboard_control_sensors_present_extended) }, \
  48. { "onboard_control_sensors_enabled_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 35, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled_extended) }, \
  49. { "onboard_control_sensors_health_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 39, offsetof(mavlink_sys_status_t, onboard_control_sensors_health_extended) }, \
  50. } \
  51. }
  52. #else
  53. #define MAVLINK_MESSAGE_INFO_SYS_STATUS { \
  54. "SYS_STATUS", \
  55. 16, \
  56. { { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \
  57. { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \
  58. { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \
  59. { "load", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_sys_status_t, load) }, \
  60. { "voltage_battery", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, voltage_battery) }, \
  61. { "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_sys_status_t, current_battery) }, \
  62. { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_sys_status_t, battery_remaining) }, \
  63. { "drop_rate_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, drop_rate_comm) }, \
  64. { "errors_comm", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_comm) }, \
  65. { "errors_count1", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_count1) }, \
  66. { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \
  67. { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \
  68. { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \
  69. { "onboard_control_sensors_present_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 31, offsetof(mavlink_sys_status_t, onboard_control_sensors_present_extended) }, \
  70. { "onboard_control_sensors_enabled_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 35, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled_extended) }, \
  71. { "onboard_control_sensors_health_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 39, offsetof(mavlink_sys_status_t, onboard_control_sensors_health_extended) }, \
  72. } \
  73. }
  74. #endif
  75. /**
  76. * @brief Pack a sys_status message
  77. * @param system_id ID of this system
  78. * @param component_id ID of this component (e.g. 200 for IMU)
  79. * @param msg The MAVLink message to compress the data into
  80. *
  81. * @param onboard_control_sensors_present Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  82. * @param onboard_control_sensors_enabled Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  83. * @param onboard_control_sensors_health Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  84. * @param load [d%] Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000
  85. * @param voltage_battery [mV] Battery voltage, UINT16_MAX: Voltage not sent by autopilot
  86. * @param current_battery [cA] Battery current, -1: Current not sent by autopilot
  87. * @param battery_remaining [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot
  88. * @param drop_rate_comm [c%] Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  89. * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  90. * @param errors_count1 Autopilot-specific errors
  91. * @param errors_count2 Autopilot-specific errors
  92. * @param errors_count3 Autopilot-specific errors
  93. * @param errors_count4 Autopilot-specific errors
  94. * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  95. * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  96. * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  97. * @return length of the message in bytes (excluding serial stream start sign)
  98. */
  99. static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  100. uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended)
  101. {
  102. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  103. char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN];
  104. _mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
  105. _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
  106. _mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
  107. _mav_put_uint16_t(buf, 12, load);
  108. _mav_put_uint16_t(buf, 14, voltage_battery);
  109. _mav_put_int16_t(buf, 16, current_battery);
  110. _mav_put_uint16_t(buf, 18, drop_rate_comm);
  111. _mav_put_uint16_t(buf, 20, errors_comm);
  112. _mav_put_uint16_t(buf, 22, errors_count1);
  113. _mav_put_uint16_t(buf, 24, errors_count2);
  114. _mav_put_uint16_t(buf, 26, errors_count3);
  115. _mav_put_uint16_t(buf, 28, errors_count4);
  116. _mav_put_int8_t(buf, 30, battery_remaining);
  117. _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended);
  118. _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended);
  119. _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended);
  120. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
  121. #else
  122. mavlink_sys_status_t packet;
  123. packet.onboard_control_sensors_present = onboard_control_sensors_present;
  124. packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled;
  125. packet.onboard_control_sensors_health = onboard_control_sensors_health;
  126. packet.load = load;
  127. packet.voltage_battery = voltage_battery;
  128. packet.current_battery = current_battery;
  129. packet.drop_rate_comm = drop_rate_comm;
  130. packet.errors_comm = errors_comm;
  131. packet.errors_count1 = errors_count1;
  132. packet.errors_count2 = errors_count2;
  133. packet.errors_count3 = errors_count3;
  134. packet.errors_count4 = errors_count4;
  135. packet.battery_remaining = battery_remaining;
  136. packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended;
  137. packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended;
  138. packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended;
  139. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
  140. #endif
  141. msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
  142. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  143. }
  144. /**
  145. * @brief Pack a sys_status message on a channel
  146. * @param system_id ID of this system
  147. * @param component_id ID of this component (e.g. 200 for IMU)
  148. * @param chan The MAVLink channel this message will be sent over
  149. * @param msg The MAVLink message to compress the data into
  150. * @param onboard_control_sensors_present Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  151. * @param onboard_control_sensors_enabled Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  152. * @param onboard_control_sensors_health Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  153. * @param load [d%] Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000
  154. * @param voltage_battery [mV] Battery voltage, UINT16_MAX: Voltage not sent by autopilot
  155. * @param current_battery [cA] Battery current, -1: Current not sent by autopilot
  156. * @param battery_remaining [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot
  157. * @param drop_rate_comm [c%] Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  158. * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  159. * @param errors_count1 Autopilot-specific errors
  160. * @param errors_count2 Autopilot-specific errors
  161. * @param errors_count3 Autopilot-specific errors
  162. * @param errors_count4 Autopilot-specific errors
  163. * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  164. * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  165. * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  166. * @return length of the message in bytes (excluding serial stream start sign)
  167. */
  168. static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  169. mavlink_message_t* msg,
  170. uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4,uint32_t onboard_control_sensors_present_extended,uint32_t onboard_control_sensors_enabled_extended,uint32_t onboard_control_sensors_health_extended)
  171. {
  172. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  173. char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN];
  174. _mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
  175. _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
  176. _mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
  177. _mav_put_uint16_t(buf, 12, load);
  178. _mav_put_uint16_t(buf, 14, voltage_battery);
  179. _mav_put_int16_t(buf, 16, current_battery);
  180. _mav_put_uint16_t(buf, 18, drop_rate_comm);
  181. _mav_put_uint16_t(buf, 20, errors_comm);
  182. _mav_put_uint16_t(buf, 22, errors_count1);
  183. _mav_put_uint16_t(buf, 24, errors_count2);
  184. _mav_put_uint16_t(buf, 26, errors_count3);
  185. _mav_put_uint16_t(buf, 28, errors_count4);
  186. _mav_put_int8_t(buf, 30, battery_remaining);
  187. _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended);
  188. _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended);
  189. _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended);
  190. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
  191. #else
  192. mavlink_sys_status_t packet;
  193. packet.onboard_control_sensors_present = onboard_control_sensors_present;
  194. packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled;
  195. packet.onboard_control_sensors_health = onboard_control_sensors_health;
  196. packet.load = load;
  197. packet.voltage_battery = voltage_battery;
  198. packet.current_battery = current_battery;
  199. packet.drop_rate_comm = drop_rate_comm;
  200. packet.errors_comm = errors_comm;
  201. packet.errors_count1 = errors_count1;
  202. packet.errors_count2 = errors_count2;
  203. packet.errors_count3 = errors_count3;
  204. packet.errors_count4 = errors_count4;
  205. packet.battery_remaining = battery_remaining;
  206. packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended;
  207. packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended;
  208. packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended;
  209. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
  210. #endif
  211. msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
  212. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  213. }
  214. /**
  215. * @brief Encode a sys_status struct
  216. *
  217. * @param system_id ID of this system
  218. * @param component_id ID of this component (e.g. 200 for IMU)
  219. * @param msg The MAVLink message to compress the data into
  220. * @param sys_status C-struct to read the message contents from
  221. */
  222. static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status)
  223. {
  224. return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended);
  225. }
  226. /**
  227. * @brief Encode a sys_status struct on a channel
  228. *
  229. * @param system_id ID of this system
  230. * @param component_id ID of this component (e.g. 200 for IMU)
  231. * @param chan The MAVLink channel this message will be sent over
  232. * @param msg The MAVLink message to compress the data into
  233. * @param sys_status C-struct to read the message contents from
  234. */
  235. static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status)
  236. {
  237. return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended);
  238. }
  239. /**
  240. * @brief Send a sys_status message
  241. * @param chan MAVLink channel to send the message
  242. *
  243. * @param onboard_control_sensors_present Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  244. * @param onboard_control_sensors_enabled Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  245. * @param onboard_control_sensors_health Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  246. * @param load [d%] Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000
  247. * @param voltage_battery [mV] Battery voltage, UINT16_MAX: Voltage not sent by autopilot
  248. * @param current_battery [cA] Battery current, -1: Current not sent by autopilot
  249. * @param battery_remaining [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot
  250. * @param drop_rate_comm [c%] Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  251. * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  252. * @param errors_count1 Autopilot-specific errors
  253. * @param errors_count2 Autopilot-specific errors
  254. * @param errors_count3 Autopilot-specific errors
  255. * @param errors_count4 Autopilot-specific errors
  256. * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  257. * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  258. * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  259. */
  260. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  261. static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended)
  262. {
  263. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  264. char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN];
  265. _mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
  266. _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
  267. _mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
  268. _mav_put_uint16_t(buf, 12, load);
  269. _mav_put_uint16_t(buf, 14, voltage_battery);
  270. _mav_put_int16_t(buf, 16, current_battery);
  271. _mav_put_uint16_t(buf, 18, drop_rate_comm);
  272. _mav_put_uint16_t(buf, 20, errors_comm);
  273. _mav_put_uint16_t(buf, 22, errors_count1);
  274. _mav_put_uint16_t(buf, 24, errors_count2);
  275. _mav_put_uint16_t(buf, 26, errors_count3);
  276. _mav_put_uint16_t(buf, 28, errors_count4);
  277. _mav_put_int8_t(buf, 30, battery_remaining);
  278. _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended);
  279. _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended);
  280. _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended);
  281. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  282. #else
  283. mavlink_sys_status_t packet;
  284. packet.onboard_control_sensors_present = onboard_control_sensors_present;
  285. packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled;
  286. packet.onboard_control_sensors_health = onboard_control_sensors_health;
  287. packet.load = load;
  288. packet.voltage_battery = voltage_battery;
  289. packet.current_battery = current_battery;
  290. packet.drop_rate_comm = drop_rate_comm;
  291. packet.errors_comm = errors_comm;
  292. packet.errors_count1 = errors_count1;
  293. packet.errors_count2 = errors_count2;
  294. packet.errors_count3 = errors_count3;
  295. packet.errors_count4 = errors_count4;
  296. packet.battery_remaining = battery_remaining;
  297. packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended;
  298. packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended;
  299. packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended;
  300. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  301. #endif
  302. }
  303. /**
  304. * @brief Send a sys_status message
  305. * @param chan MAVLink channel to send the message
  306. * @param struct The MAVLink struct to serialize
  307. */
  308. static inline void mavlink_msg_sys_status_send_struct(mavlink_channel_t chan, const mavlink_sys_status_t* sys_status)
  309. {
  310. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  311. mavlink_msg_sys_status_send(chan, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended);
  312. #else
  313. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)sys_status, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  314. #endif
  315. }
  316. #if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  317. /*
  318. This variant of _send() can be used to save stack space by re-using
  319. memory from the receive buffer. The caller provides a
  320. mavlink_message_t which is the size of a full mavlink message. This
  321. is usually the receive buffer for the channel, and allows a reply to an
  322. incoming message with minimum stack space usage.
  323. */
  324. static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended)
  325. {
  326. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  327. char *buf = (char *)msgbuf;
  328. _mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
  329. _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
  330. _mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
  331. _mav_put_uint16_t(buf, 12, load);
  332. _mav_put_uint16_t(buf, 14, voltage_battery);
  333. _mav_put_int16_t(buf, 16, current_battery);
  334. _mav_put_uint16_t(buf, 18, drop_rate_comm);
  335. _mav_put_uint16_t(buf, 20, errors_comm);
  336. _mav_put_uint16_t(buf, 22, errors_count1);
  337. _mav_put_uint16_t(buf, 24, errors_count2);
  338. _mav_put_uint16_t(buf, 26, errors_count3);
  339. _mav_put_uint16_t(buf, 28, errors_count4);
  340. _mav_put_int8_t(buf, 30, battery_remaining);
  341. _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended);
  342. _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended);
  343. _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended);
  344. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  345. #else
  346. mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf;
  347. packet->onboard_control_sensors_present = onboard_control_sensors_present;
  348. packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled;
  349. packet->onboard_control_sensors_health = onboard_control_sensors_health;
  350. packet->load = load;
  351. packet->voltage_battery = voltage_battery;
  352. packet->current_battery = current_battery;
  353. packet->drop_rate_comm = drop_rate_comm;
  354. packet->errors_comm = errors_comm;
  355. packet->errors_count1 = errors_count1;
  356. packet->errors_count2 = errors_count2;
  357. packet->errors_count3 = errors_count3;
  358. packet->errors_count4 = errors_count4;
  359. packet->battery_remaining = battery_remaining;
  360. packet->onboard_control_sensors_present_extended = onboard_control_sensors_present_extended;
  361. packet->onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended;
  362. packet->onboard_control_sensors_health_extended = onboard_control_sensors_health_extended;
  363. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
  364. #endif
  365. }
  366. #endif
  367. #endif
  368. // MESSAGE SYS_STATUS UNPACKING
  369. /**
  370. * @brief Get field onboard_control_sensors_present from sys_status message
  371. *
  372. * @return Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  373. */
  374. static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg)
  375. {
  376. return _MAV_RETURN_uint32_t(msg, 0);
  377. }
  378. /**
  379. * @brief Get field onboard_control_sensors_enabled from sys_status message
  380. *
  381. * @return Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  382. */
  383. static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg)
  384. {
  385. return _MAV_RETURN_uint32_t(msg, 4);
  386. }
  387. /**
  388. * @brief Get field onboard_control_sensors_health from sys_status message
  389. *
  390. * @return Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  391. */
  392. static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg)
  393. {
  394. return _MAV_RETURN_uint32_t(msg, 8);
  395. }
  396. /**
  397. * @brief Get field load from sys_status message
  398. *
  399. * @return [d%] Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000
  400. */
  401. static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg)
  402. {
  403. return _MAV_RETURN_uint16_t(msg, 12);
  404. }
  405. /**
  406. * @brief Get field voltage_battery from sys_status message
  407. *
  408. * @return [mV] Battery voltage, UINT16_MAX: Voltage not sent by autopilot
  409. */
  410. static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg)
  411. {
  412. return _MAV_RETURN_uint16_t(msg, 14);
  413. }
  414. /**
  415. * @brief Get field current_battery from sys_status message
  416. *
  417. * @return [cA] Battery current, -1: Current not sent by autopilot
  418. */
  419. static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg)
  420. {
  421. return _MAV_RETURN_int16_t(msg, 16);
  422. }
  423. /**
  424. * @brief Get field battery_remaining from sys_status message
  425. *
  426. * @return [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot
  427. */
  428. static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg)
  429. {
  430. return _MAV_RETURN_int8_t(msg, 30);
  431. }
  432. /**
  433. * @brief Get field drop_rate_comm from sys_status message
  434. *
  435. * @return [c%] Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  436. */
  437. static inline uint16_t mavlink_msg_sys_status_get_drop_rate_comm(const mavlink_message_t* msg)
  438. {
  439. return _MAV_RETURN_uint16_t(msg, 18);
  440. }
  441. /**
  442. * @brief Get field errors_comm from sys_status message
  443. *
  444. * @return Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
  445. */
  446. static inline uint16_t mavlink_msg_sys_status_get_errors_comm(const mavlink_message_t* msg)
  447. {
  448. return _MAV_RETURN_uint16_t(msg, 20);
  449. }
  450. /**
  451. * @brief Get field errors_count1 from sys_status message
  452. *
  453. * @return Autopilot-specific errors
  454. */
  455. static inline uint16_t mavlink_msg_sys_status_get_errors_count1(const mavlink_message_t* msg)
  456. {
  457. return _MAV_RETURN_uint16_t(msg, 22);
  458. }
  459. /**
  460. * @brief Get field errors_count2 from sys_status message
  461. *
  462. * @return Autopilot-specific errors
  463. */
  464. static inline uint16_t mavlink_msg_sys_status_get_errors_count2(const mavlink_message_t* msg)
  465. {
  466. return _MAV_RETURN_uint16_t(msg, 24);
  467. }
  468. /**
  469. * @brief Get field errors_count3 from sys_status message
  470. *
  471. * @return Autopilot-specific errors
  472. */
  473. static inline uint16_t mavlink_msg_sys_status_get_errors_count3(const mavlink_message_t* msg)
  474. {
  475. return _MAV_RETURN_uint16_t(msg, 26);
  476. }
  477. /**
  478. * @brief Get field errors_count4 from sys_status message
  479. *
  480. * @return Autopilot-specific errors
  481. */
  482. static inline uint16_t mavlink_msg_sys_status_get_errors_count4(const mavlink_message_t* msg)
  483. {
  484. return _MAV_RETURN_uint16_t(msg, 28);
  485. }
  486. /**
  487. * @brief Get field onboard_control_sensors_present_extended from sys_status message
  488. *
  489. * @return Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.
  490. */
  491. static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present_extended(const mavlink_message_t* msg)
  492. {
  493. return _MAV_RETURN_uint32_t(msg, 31);
  494. }
  495. /**
  496. * @brief Get field onboard_control_sensors_enabled_extended from sys_status message
  497. *
  498. * @return Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.
  499. */
  500. static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled_extended(const mavlink_message_t* msg)
  501. {
  502. return _MAV_RETURN_uint32_t(msg, 35);
  503. }
  504. /**
  505. * @brief Get field onboard_control_sensors_health_extended from sys_status message
  506. *
  507. * @return Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.
  508. */
  509. static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health_extended(const mavlink_message_t* msg)
  510. {
  511. return _MAV_RETURN_uint32_t(msg, 39);
  512. }
  513. /**
  514. * @brief Decode a sys_status message into a struct
  515. *
  516. * @param msg The message to decode
  517. * @param sys_status C-struct to decode the message contents into
  518. */
  519. static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
  520. {
  521. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  522. sys_status->onboard_control_sensors_present = mavlink_msg_sys_status_get_onboard_control_sensors_present(msg);
  523. sys_status->onboard_control_sensors_enabled = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(msg);
  524. sys_status->onboard_control_sensors_health = mavlink_msg_sys_status_get_onboard_control_sensors_health(msg);
  525. sys_status->load = mavlink_msg_sys_status_get_load(msg);
  526. sys_status->voltage_battery = mavlink_msg_sys_status_get_voltage_battery(msg);
  527. sys_status->current_battery = mavlink_msg_sys_status_get_current_battery(msg);
  528. sys_status->drop_rate_comm = mavlink_msg_sys_status_get_drop_rate_comm(msg);
  529. sys_status->errors_comm = mavlink_msg_sys_status_get_errors_comm(msg);
  530. sys_status->errors_count1 = mavlink_msg_sys_status_get_errors_count1(msg);
  531. sys_status->errors_count2 = mavlink_msg_sys_status_get_errors_count2(msg);
  532. sys_status->errors_count3 = mavlink_msg_sys_status_get_errors_count3(msg);
  533. sys_status->errors_count4 = mavlink_msg_sys_status_get_errors_count4(msg);
  534. sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg);
  535. sys_status->onboard_control_sensors_present_extended = mavlink_msg_sys_status_get_onboard_control_sensors_present_extended(msg);
  536. sys_status->onboard_control_sensors_enabled_extended = mavlink_msg_sys_status_get_onboard_control_sensors_enabled_extended(msg);
  537. sys_status->onboard_control_sensors_health_extended = mavlink_msg_sys_status_get_onboard_control_sensors_health_extended(msg);
  538. #else
  539. uint8_t len = msg->len < MAVLINK_MSG_ID_SYS_STATUS_LEN? msg->len : MAVLINK_MSG_ID_SYS_STATUS_LEN;
  540. memset(sys_status, 0, MAVLINK_MSG_ID_SYS_STATUS_LEN);
  541. memcpy(sys_status, _MAV_PAYLOAD(msg), len);
  542. #endif
  543. }