mavlink_msg_obstacle_distance.h 24 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405
  1. #pragma once
  2. // MESSAGE OBSTACLE_DISTANCE PACKING
  3. #define MAVLINK_MSG_ID_OBSTACLE_DISTANCE 330
  4. MAVPACKED(
  5. typedef struct __mavlink_obstacle_distance_t {
  6. uint64_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 distances[72]; /*< [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.*/
  8. uint16_t min_distance; /*< [cm] Minimum distance the sensor can measure.*/
  9. uint16_t max_distance; /*< [cm] Maximum distance the sensor can measure.*/
  10. uint8_t sensor_type; /*< Class id of the distance sensor type.*/
  11. uint8_t increment; /*< [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.*/
  12. float increment_f; /*< [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.*/
  13. float angle_offset; /*< [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.*/
  14. uint8_t frame; /*< Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.*/
  15. }) mavlink_obstacle_distance_t;
  16. #define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN 167
  17. #define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN 158
  18. #define MAVLINK_MSG_ID_330_LEN 167
  19. #define MAVLINK_MSG_ID_330_MIN_LEN 158
  20. #define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC 23
  21. #define MAVLINK_MSG_ID_330_CRC 23
  22. #define MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN 72
  23. #if MAVLINK_COMMAND_24BIT
  24. #define MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE { \
  25. 330, \
  26. "OBSTACLE_DISTANCE", \
  27. 9, \
  28. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_obstacle_distance_t, time_usec) }, \
  29. { "sensor_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 156, offsetof(mavlink_obstacle_distance_t, sensor_type) }, \
  30. { "distances", NULL, MAVLINK_TYPE_UINT16_T, 72, 8, offsetof(mavlink_obstacle_distance_t, distances) }, \
  31. { "increment", NULL, MAVLINK_TYPE_UINT8_T, 0, 157, offsetof(mavlink_obstacle_distance_t, increment) }, \
  32. { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 152, offsetof(mavlink_obstacle_distance_t, min_distance) }, \
  33. { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 154, offsetof(mavlink_obstacle_distance_t, max_distance) }, \
  34. { "increment_f", NULL, MAVLINK_TYPE_FLOAT, 0, 158, offsetof(mavlink_obstacle_distance_t, increment_f) }, \
  35. { "angle_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 162, offsetof(mavlink_obstacle_distance_t, angle_offset) }, \
  36. { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 166, offsetof(mavlink_obstacle_distance_t, frame) }, \
  37. } \
  38. }
  39. #else
  40. #define MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE { \
  41. "OBSTACLE_DISTANCE", \
  42. 9, \
  43. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_obstacle_distance_t, time_usec) }, \
  44. { "sensor_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 156, offsetof(mavlink_obstacle_distance_t, sensor_type) }, \
  45. { "distances", NULL, MAVLINK_TYPE_UINT16_T, 72, 8, offsetof(mavlink_obstacle_distance_t, distances) }, \
  46. { "increment", NULL, MAVLINK_TYPE_UINT8_T, 0, 157, offsetof(mavlink_obstacle_distance_t, increment) }, \
  47. { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 152, offsetof(mavlink_obstacle_distance_t, min_distance) }, \
  48. { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 154, offsetof(mavlink_obstacle_distance_t, max_distance) }, \
  49. { "increment_f", NULL, MAVLINK_TYPE_FLOAT, 0, 158, offsetof(mavlink_obstacle_distance_t, increment_f) }, \
  50. { "angle_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 162, offsetof(mavlink_obstacle_distance_t, angle_offset) }, \
  51. { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 166, offsetof(mavlink_obstacle_distance_t, frame) }, \
  52. } \
  53. }
  54. #endif
  55. /**
  56. * @brief Pack a obstacle_distance message
  57. * @param system_id ID of this system
  58. * @param component_id ID of this component (e.g. 200 for IMU)
  59. * @param msg The MAVLink message to compress the data into
  60. *
  61. * @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.
  62. * @param sensor_type Class id of the distance sensor type.
  63. * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.
  64. * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.
  65. * @param min_distance [cm] Minimum distance the sensor can measure.
  66. * @param max_distance [cm] Maximum distance the sensor can measure.
  67. * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.
  68. * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.
  69. * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.
  70. * @return length of the message in bytes (excluding serial stream start sign)
  71. */
  72. static inline uint16_t mavlink_msg_obstacle_distance_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  73. uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame)
  74. {
  75. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  76. char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN];
  77. _mav_put_uint64_t(buf, 0, time_usec);
  78. _mav_put_uint16_t(buf, 152, min_distance);
  79. _mav_put_uint16_t(buf, 154, max_distance);
  80. _mav_put_uint8_t(buf, 156, sensor_type);
  81. _mav_put_uint8_t(buf, 157, increment);
  82. _mav_put_float(buf, 158, increment_f);
  83. _mav_put_float(buf, 162, angle_offset);
  84. _mav_put_uint8_t(buf, 166, frame);
  85. _mav_put_uint16_t_array(buf, 8, distances, 72);
  86. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN);
  87. #else
  88. mavlink_obstacle_distance_t packet;
  89. packet.time_usec = time_usec;
  90. packet.min_distance = min_distance;
  91. packet.max_distance = max_distance;
  92. packet.sensor_type = sensor_type;
  93. packet.increment = increment;
  94. packet.increment_f = increment_f;
  95. packet.angle_offset = angle_offset;
  96. packet.frame = frame;
  97. mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72);
  98. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN);
  99. #endif
  100. msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE;
  101. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  102. }
  103. /**
  104. * @brief Pack a obstacle_distance message on a channel
  105. * @param system_id ID of this system
  106. * @param component_id ID of this component (e.g. 200 for IMU)
  107. * @param chan The MAVLink channel this message will be sent over
  108. * @param msg The MAVLink message to compress the data into
  109. * @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.
  110. * @param sensor_type Class id of the distance sensor type.
  111. * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.
  112. * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.
  113. * @param min_distance [cm] Minimum distance the sensor can measure.
  114. * @param max_distance [cm] Maximum distance the sensor can measure.
  115. * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.
  116. * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.
  117. * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.
  118. * @return length of the message in bytes (excluding serial stream start sign)
  119. */
  120. static inline uint16_t mavlink_msg_obstacle_distance_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  121. mavlink_message_t* msg,
  122. uint64_t time_usec,uint8_t sensor_type,const uint16_t *distances,uint8_t increment,uint16_t min_distance,uint16_t max_distance,float increment_f,float angle_offset,uint8_t frame)
  123. {
  124. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  125. char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN];
  126. _mav_put_uint64_t(buf, 0, time_usec);
  127. _mav_put_uint16_t(buf, 152, min_distance);
  128. _mav_put_uint16_t(buf, 154, max_distance);
  129. _mav_put_uint8_t(buf, 156, sensor_type);
  130. _mav_put_uint8_t(buf, 157, increment);
  131. _mav_put_float(buf, 158, increment_f);
  132. _mav_put_float(buf, 162, angle_offset);
  133. _mav_put_uint8_t(buf, 166, frame);
  134. _mav_put_uint16_t_array(buf, 8, distances, 72);
  135. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN);
  136. #else
  137. mavlink_obstacle_distance_t packet;
  138. packet.time_usec = time_usec;
  139. packet.min_distance = min_distance;
  140. packet.max_distance = max_distance;
  141. packet.sensor_type = sensor_type;
  142. packet.increment = increment;
  143. packet.increment_f = increment_f;
  144. packet.angle_offset = angle_offset;
  145. packet.frame = frame;
  146. mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72);
  147. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN);
  148. #endif
  149. msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE;
  150. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  151. }
  152. /**
  153. * @brief Encode a obstacle_distance struct
  154. *
  155. * @param system_id ID of this system
  156. * @param component_id ID of this component (e.g. 200 for IMU)
  157. * @param msg The MAVLink message to compress the data into
  158. * @param obstacle_distance C-struct to read the message contents from
  159. */
  160. static inline uint16_t mavlink_msg_obstacle_distance_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance)
  161. {
  162. return mavlink_msg_obstacle_distance_pack(system_id, component_id, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame);
  163. }
  164. /**
  165. * @brief Encode a obstacle_distance struct on a channel
  166. *
  167. * @param system_id ID of this system
  168. * @param component_id ID of this component (e.g. 200 for IMU)
  169. * @param chan The MAVLink channel this message will be sent over
  170. * @param msg The MAVLink message to compress the data into
  171. * @param obstacle_distance C-struct to read the message contents from
  172. */
  173. static inline uint16_t mavlink_msg_obstacle_distance_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance)
  174. {
  175. return mavlink_msg_obstacle_distance_pack_chan(system_id, component_id, chan, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame);
  176. }
  177. /**
  178. * @brief Send a obstacle_distance message
  179. * @param chan MAVLink channel to send the message
  180. *
  181. * @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.
  182. * @param sensor_type Class id of the distance sensor type.
  183. * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.
  184. * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.
  185. * @param min_distance [cm] Minimum distance the sensor can measure.
  186. * @param max_distance [cm] Maximum distance the sensor can measure.
  187. * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.
  188. * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.
  189. * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.
  190. */
  191. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  192. static inline void mavlink_msg_obstacle_distance_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame)
  193. {
  194. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  195. char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN];
  196. _mav_put_uint64_t(buf, 0, time_usec);
  197. _mav_put_uint16_t(buf, 152, min_distance);
  198. _mav_put_uint16_t(buf, 154, max_distance);
  199. _mav_put_uint8_t(buf, 156, sensor_type);
  200. _mav_put_uint8_t(buf, 157, increment);
  201. _mav_put_float(buf, 158, increment_f);
  202. _mav_put_float(buf, 162, angle_offset);
  203. _mav_put_uint8_t(buf, 166, frame);
  204. _mav_put_uint16_t_array(buf, 8, distances, 72);
  205. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  206. #else
  207. mavlink_obstacle_distance_t packet;
  208. packet.time_usec = time_usec;
  209. packet.min_distance = min_distance;
  210. packet.max_distance = max_distance;
  211. packet.sensor_type = sensor_type;
  212. packet.increment = increment;
  213. packet.increment_f = increment_f;
  214. packet.angle_offset = angle_offset;
  215. packet.frame = frame;
  216. mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72);
  217. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  218. #endif
  219. }
  220. /**
  221. * @brief Send a obstacle_distance message
  222. * @param chan MAVLink channel to send the message
  223. * @param struct The MAVLink struct to serialize
  224. */
  225. static inline void mavlink_msg_obstacle_distance_send_struct(mavlink_channel_t chan, const mavlink_obstacle_distance_t* obstacle_distance)
  226. {
  227. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  228. mavlink_msg_obstacle_distance_send(chan, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame);
  229. #else
  230. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)obstacle_distance, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  231. #endif
  232. }
  233. #if MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  234. /*
  235. This variant of _send() can be used to save stack space by re-using
  236. memory from the receive buffer. The caller provides a
  237. mavlink_message_t which is the size of a full mavlink message. This
  238. is usually the receive buffer for the channel, and allows a reply to an
  239. incoming message with minimum stack space usage.
  240. */
  241. static inline void mavlink_msg_obstacle_distance_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame)
  242. {
  243. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  244. char *buf = (char *)msgbuf;
  245. _mav_put_uint64_t(buf, 0, time_usec);
  246. _mav_put_uint16_t(buf, 152, min_distance);
  247. _mav_put_uint16_t(buf, 154, max_distance);
  248. _mav_put_uint8_t(buf, 156, sensor_type);
  249. _mav_put_uint8_t(buf, 157, increment);
  250. _mav_put_float(buf, 158, increment_f);
  251. _mav_put_float(buf, 162, angle_offset);
  252. _mav_put_uint8_t(buf, 166, frame);
  253. _mav_put_uint16_t_array(buf, 8, distances, 72);
  254. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  255. #else
  256. mavlink_obstacle_distance_t *packet = (mavlink_obstacle_distance_t *)msgbuf;
  257. packet->time_usec = time_usec;
  258. packet->min_distance = min_distance;
  259. packet->max_distance = max_distance;
  260. packet->sensor_type = sensor_type;
  261. packet->increment = increment;
  262. packet->increment_f = increment_f;
  263. packet->angle_offset = angle_offset;
  264. packet->frame = frame;
  265. mav_array_memcpy(packet->distances, distances, sizeof(uint16_t)*72);
  266. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC);
  267. #endif
  268. }
  269. #endif
  270. #endif
  271. // MESSAGE OBSTACLE_DISTANCE UNPACKING
  272. /**
  273. * @brief Get field time_usec from obstacle_distance message
  274. *
  275. * @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.
  276. */
  277. static inline uint64_t mavlink_msg_obstacle_distance_get_time_usec(const mavlink_message_t* msg)
  278. {
  279. return _MAV_RETURN_uint64_t(msg, 0);
  280. }
  281. /**
  282. * @brief Get field sensor_type from obstacle_distance message
  283. *
  284. * @return Class id of the distance sensor type.
  285. */
  286. static inline uint8_t mavlink_msg_obstacle_distance_get_sensor_type(const mavlink_message_t* msg)
  287. {
  288. return _MAV_RETURN_uint8_t(msg, 156);
  289. }
  290. /**
  291. * @brief Get field distances from obstacle_distance message
  292. *
  293. * @return [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.
  294. */
  295. static inline uint16_t mavlink_msg_obstacle_distance_get_distances(const mavlink_message_t* msg, uint16_t *distances)
  296. {
  297. return _MAV_RETURN_uint16_t_array(msg, distances, 72, 8);
  298. }
  299. /**
  300. * @brief Get field increment from obstacle_distance message
  301. *
  302. * @return [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.
  303. */
  304. static inline uint8_t mavlink_msg_obstacle_distance_get_increment(const mavlink_message_t* msg)
  305. {
  306. return _MAV_RETURN_uint8_t(msg, 157);
  307. }
  308. /**
  309. * @brief Get field min_distance from obstacle_distance message
  310. *
  311. * @return [cm] Minimum distance the sensor can measure.
  312. */
  313. static inline uint16_t mavlink_msg_obstacle_distance_get_min_distance(const mavlink_message_t* msg)
  314. {
  315. return _MAV_RETURN_uint16_t(msg, 152);
  316. }
  317. /**
  318. * @brief Get field max_distance from obstacle_distance message
  319. *
  320. * @return [cm] Maximum distance the sensor can measure.
  321. */
  322. static inline uint16_t mavlink_msg_obstacle_distance_get_max_distance(const mavlink_message_t* msg)
  323. {
  324. return _MAV_RETURN_uint16_t(msg, 154);
  325. }
  326. /**
  327. * @brief Get field increment_f from obstacle_distance message
  328. *
  329. * @return [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.
  330. */
  331. static inline float mavlink_msg_obstacle_distance_get_increment_f(const mavlink_message_t* msg)
  332. {
  333. return _MAV_RETURN_float(msg, 158);
  334. }
  335. /**
  336. * @brief Get field angle_offset from obstacle_distance message
  337. *
  338. * @return [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.
  339. */
  340. static inline float mavlink_msg_obstacle_distance_get_angle_offset(const mavlink_message_t* msg)
  341. {
  342. return _MAV_RETURN_float(msg, 162);
  343. }
  344. /**
  345. * @brief Get field frame from obstacle_distance message
  346. *
  347. * @return Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.
  348. */
  349. static inline uint8_t mavlink_msg_obstacle_distance_get_frame(const mavlink_message_t* msg)
  350. {
  351. return _MAV_RETURN_uint8_t(msg, 166);
  352. }
  353. /**
  354. * @brief Decode a obstacle_distance message into a struct
  355. *
  356. * @param msg The message to decode
  357. * @param obstacle_distance C-struct to decode the message contents into
  358. */
  359. static inline void mavlink_msg_obstacle_distance_decode(const mavlink_message_t* msg, mavlink_obstacle_distance_t* obstacle_distance)
  360. {
  361. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  362. obstacle_distance->time_usec = mavlink_msg_obstacle_distance_get_time_usec(msg);
  363. mavlink_msg_obstacle_distance_get_distances(msg, obstacle_distance->distances);
  364. obstacle_distance->min_distance = mavlink_msg_obstacle_distance_get_min_distance(msg);
  365. obstacle_distance->max_distance = mavlink_msg_obstacle_distance_get_max_distance(msg);
  366. obstacle_distance->sensor_type = mavlink_msg_obstacle_distance_get_sensor_type(msg);
  367. obstacle_distance->increment = mavlink_msg_obstacle_distance_get_increment(msg);
  368. obstacle_distance->increment_f = mavlink_msg_obstacle_distance_get_increment_f(msg);
  369. obstacle_distance->angle_offset = mavlink_msg_obstacle_distance_get_angle_offset(msg);
  370. obstacle_distance->frame = mavlink_msg_obstacle_distance_get_frame(msg);
  371. #else
  372. uint8_t len = msg->len < MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN? msg->len : MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN;
  373. memset(obstacle_distance, 0, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN);
  374. memcpy(obstacle_distance, _MAV_PAYLOAD(msg), len);
  375. #endif
  376. }