mavlink_msg_local_position_ned_cov.h 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558
  1. #pragma once
  2. // MESSAGE LOCAL_POSITION_NED_COV PACKING
  3. #define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV 64
  4. typedef struct __mavlink_local_position_ned_cov_t {
  5. 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.*/
  6. float x; /*< [m] X Position*/
  7. float y; /*< [m] Y Position*/
  8. float z; /*< [m] Z Position*/
  9. float vx; /*< [m/s] X Speed*/
  10. float vy; /*< [m/s] Y Speed*/
  11. float vz; /*< [m/s] Z Speed*/
  12. float ax; /*< [m/s/s] X Acceleration*/
  13. float ay; /*< [m/s/s] Y Acceleration*/
  14. float az; /*< [m/s/s] Z Acceleration*/
  15. float covariance[45]; /*< Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array.*/
  16. uint8_t estimator_type; /*< Class id of the estimator this estimate originated from.*/
  17. } mavlink_local_position_ned_cov_t;
  18. #define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN 225
  19. #define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN 225
  20. #define MAVLINK_MSG_ID_64_LEN 225
  21. #define MAVLINK_MSG_ID_64_MIN_LEN 225
  22. #define MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC 191
  23. #define MAVLINK_MSG_ID_64_CRC 191
  24. #define MAVLINK_MSG_LOCAL_POSITION_NED_COV_FIELD_COVARIANCE_LEN 45
  25. #if MAVLINK_COMMAND_24BIT
  26. #define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV { \
  27. 64, \
  28. "LOCAL_POSITION_NED_COV", \
  29. 12, \
  30. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_ned_cov_t, time_usec) }, \
  31. { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 224, offsetof(mavlink_local_position_ned_cov_t, estimator_type) }, \
  32. { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_cov_t, x) }, \
  33. { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_cov_t, y) }, \
  34. { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_cov_t, z) }, \
  35. { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_cov_t, vx) }, \
  36. { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_cov_t, vy) }, \
  37. { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_ned_cov_t, vz) }, \
  38. { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_position_ned_cov_t, ax) }, \
  39. { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_local_position_ned_cov_t, ay) }, \
  40. { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_local_position_ned_cov_t, az) }, \
  41. { "covariance", NULL, MAVLINK_TYPE_FLOAT, 45, 44, offsetof(mavlink_local_position_ned_cov_t, covariance) }, \
  42. } \
  43. }
  44. #else
  45. #define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV { \
  46. "LOCAL_POSITION_NED_COV", \
  47. 12, \
  48. { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_ned_cov_t, time_usec) }, \
  49. { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 224, offsetof(mavlink_local_position_ned_cov_t, estimator_type) }, \
  50. { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_ned_cov_t, x) }, \
  51. { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_ned_cov_t, y) }, \
  52. { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_ned_cov_t, z) }, \
  53. { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_ned_cov_t, vx) }, \
  54. { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_ned_cov_t, vy) }, \
  55. { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_ned_cov_t, vz) }, \
  56. { "ax", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_local_position_ned_cov_t, ax) }, \
  57. { "ay", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_local_position_ned_cov_t, ay) }, \
  58. { "az", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_local_position_ned_cov_t, az) }, \
  59. { "covariance", NULL, MAVLINK_TYPE_FLOAT, 45, 44, offsetof(mavlink_local_position_ned_cov_t, covariance) }, \
  60. } \
  61. }
  62. #endif
  63. /**
  64. * @brief Pack a local_position_ned_cov message
  65. * @param system_id ID of this system
  66. * @param component_id ID of this component (e.g. 200 for IMU)
  67. * @param msg The MAVLink message to compress the data into
  68. *
  69. * @param 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.
  70. * @param estimator_type Class id of the estimator this estimate originated from.
  71. * @param x [m] X Position
  72. * @param y [m] Y Position
  73. * @param z [m] Z Position
  74. * @param vx [m/s] X Speed
  75. * @param vy [m/s] Y Speed
  76. * @param vz [m/s] Z Speed
  77. * @param ax [m/s/s] X Acceleration
  78. * @param ay [m/s/s] Y Acceleration
  79. * @param az [m/s/s] Z Acceleration
  80. * @param covariance Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array.
  81. * @return length of the message in bytes (excluding serial stream start sign)
  82. */
  83. static inline uint16_t mavlink_msg_local_position_ned_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
  84. uint64_t time_usec, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance)
  85. {
  86. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  87. char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN];
  88. _mav_put_uint64_t(buf, 0, time_usec);
  89. _mav_put_float(buf, 8, x);
  90. _mav_put_float(buf, 12, y);
  91. _mav_put_float(buf, 16, z);
  92. _mav_put_float(buf, 20, vx);
  93. _mav_put_float(buf, 24, vy);
  94. _mav_put_float(buf, 28, vz);
  95. _mav_put_float(buf, 32, ax);
  96. _mav_put_float(buf, 36, ay);
  97. _mav_put_float(buf, 40, az);
  98. _mav_put_uint8_t(buf, 224, estimator_type);
  99. _mav_put_float_array(buf, 44, covariance, 45);
  100. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  101. #else
  102. mavlink_local_position_ned_cov_t packet;
  103. packet.time_usec = time_usec;
  104. packet.x = x;
  105. packet.y = y;
  106. packet.z = z;
  107. packet.vx = vx;
  108. packet.vy = vy;
  109. packet.vz = vz;
  110. packet.ax = ax;
  111. packet.ay = ay;
  112. packet.az = az;
  113. packet.estimator_type = estimator_type;
  114. mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45);
  115. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  116. #endif
  117. msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
  118. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  119. }
  120. /**
  121. * @brief Pack a local_position_ned_cov message
  122. * @param system_id ID of this system
  123. * @param component_id ID of this component (e.g. 200 for IMU)
  124. * @param status MAVLink status structure
  125. * @param msg The MAVLink message to compress the data into
  126. *
  127. * @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.
  128. * @param estimator_type Class id of the estimator this estimate originated from.
  129. * @param x [m] X Position
  130. * @param y [m] Y Position
  131. * @param z [m] Z Position
  132. * @param vx [m/s] X Speed
  133. * @param vy [m/s] Y Speed
  134. * @param vz [m/s] Z Speed
  135. * @param ax [m/s/s] X Acceleration
  136. * @param ay [m/s/s] Y Acceleration
  137. * @param az [m/s/s] Z Acceleration
  138. * @param covariance Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array.
  139. * @return length of the message in bytes (excluding serial stream start sign)
  140. */
  141. static inline uint16_t mavlink_msg_local_position_ned_cov_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
  142. uint64_t time_usec, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance)
  143. {
  144. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  145. char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN];
  146. _mav_put_uint64_t(buf, 0, time_usec);
  147. _mav_put_float(buf, 8, x);
  148. _mav_put_float(buf, 12, y);
  149. _mav_put_float(buf, 16, z);
  150. _mav_put_float(buf, 20, vx);
  151. _mav_put_float(buf, 24, vy);
  152. _mav_put_float(buf, 28, vz);
  153. _mav_put_float(buf, 32, ax);
  154. _mav_put_float(buf, 36, ay);
  155. _mav_put_float(buf, 40, az);
  156. _mav_put_uint8_t(buf, 224, estimator_type);
  157. _mav_put_float_array(buf, 44, covariance, 45);
  158. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  159. #else
  160. mavlink_local_position_ned_cov_t packet;
  161. packet.time_usec = time_usec;
  162. packet.x = x;
  163. packet.y = y;
  164. packet.z = z;
  165. packet.vx = vx;
  166. packet.vy = vy;
  167. packet.vz = vz;
  168. packet.ax = ax;
  169. packet.ay = ay;
  170. packet.az = az;
  171. packet.estimator_type = estimator_type;
  172. mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45);
  173. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  174. #endif
  175. msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
  176. #if MAVLINK_CRC_EXTRA
  177. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  178. #else
  179. return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  180. #endif
  181. }
  182. /**
  183. * @brief Pack a local_position_ned_cov message on a channel
  184. * @param system_id ID of this system
  185. * @param component_id ID of this component (e.g. 200 for IMU)
  186. * @param chan The MAVLink channel this message will be sent over
  187. * @param msg The MAVLink message to compress the data into
  188. * @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.
  189. * @param estimator_type Class id of the estimator this estimate originated from.
  190. * @param x [m] X Position
  191. * @param y [m] Y Position
  192. * @param z [m] Z Position
  193. * @param vx [m/s] X Speed
  194. * @param vy [m/s] Y Speed
  195. * @param vz [m/s] Z Speed
  196. * @param ax [m/s/s] X Acceleration
  197. * @param ay [m/s/s] Y Acceleration
  198. * @param az [m/s/s] Z Acceleration
  199. * @param covariance Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array.
  200. * @return length of the message in bytes (excluding serial stream start sign)
  201. */
  202. static inline uint16_t mavlink_msg_local_position_ned_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
  203. mavlink_message_t* msg,
  204. uint64_t time_usec,uint8_t estimator_type,float x,float y,float z,float vx,float vy,float vz,float ax,float ay,float az,const float *covariance)
  205. {
  206. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  207. char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN];
  208. _mav_put_uint64_t(buf, 0, time_usec);
  209. _mav_put_float(buf, 8, x);
  210. _mav_put_float(buf, 12, y);
  211. _mav_put_float(buf, 16, z);
  212. _mav_put_float(buf, 20, vx);
  213. _mav_put_float(buf, 24, vy);
  214. _mav_put_float(buf, 28, vz);
  215. _mav_put_float(buf, 32, ax);
  216. _mav_put_float(buf, 36, ay);
  217. _mav_put_float(buf, 40, az);
  218. _mav_put_uint8_t(buf, 224, estimator_type);
  219. _mav_put_float_array(buf, 44, covariance, 45);
  220. memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  221. #else
  222. mavlink_local_position_ned_cov_t packet;
  223. packet.time_usec = time_usec;
  224. packet.x = x;
  225. packet.y = y;
  226. packet.z = z;
  227. packet.vx = vx;
  228. packet.vy = vy;
  229. packet.vz = vz;
  230. packet.ax = ax;
  231. packet.ay = ay;
  232. packet.az = az;
  233. packet.estimator_type = estimator_type;
  234. mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45);
  235. memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  236. #endif
  237. msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
  238. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  239. }
  240. /**
  241. * @brief Encode a local_position_ned_cov struct
  242. *
  243. * @param system_id ID of this system
  244. * @param component_id ID of this component (e.g. 200 for IMU)
  245. * @param msg The MAVLink message to compress the data into
  246. * @param local_position_ned_cov C-struct to read the message contents from
  247. */
  248. static inline uint16_t mavlink_msg_local_position_ned_cov_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov)
  249. {
  250. return mavlink_msg_local_position_ned_cov_pack(system_id, component_id, msg, local_position_ned_cov->time_usec, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance);
  251. }
  252. /**
  253. * @brief Encode a local_position_ned_cov struct on a channel
  254. *
  255. * @param system_id ID of this system
  256. * @param component_id ID of this component (e.g. 200 for IMU)
  257. * @param chan The MAVLink channel this message will be sent over
  258. * @param msg The MAVLink message to compress the data into
  259. * @param local_position_ned_cov C-struct to read the message contents from
  260. */
  261. static inline uint16_t mavlink_msg_local_position_ned_cov_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov)
  262. {
  263. return mavlink_msg_local_position_ned_cov_pack_chan(system_id, component_id, chan, msg, local_position_ned_cov->time_usec, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance);
  264. }
  265. /**
  266. * @brief Encode a local_position_ned_cov struct with provided status structure
  267. *
  268. * @param system_id ID of this system
  269. * @param component_id ID of this component (e.g. 200 for IMU)
  270. * @param status MAVLink status structure
  271. * @param msg The MAVLink message to compress the data into
  272. * @param local_position_ned_cov C-struct to read the message contents from
  273. */
  274. static inline uint16_t mavlink_msg_local_position_ned_cov_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov)
  275. {
  276. return mavlink_msg_local_position_ned_cov_pack_status(system_id, component_id, _status, msg, local_position_ned_cov->time_usec, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance);
  277. }
  278. /**
  279. * @brief Send a local_position_ned_cov message
  280. * @param chan MAVLink channel to send the message
  281. *
  282. * @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.
  283. * @param estimator_type Class id of the estimator this estimate originated from.
  284. * @param x [m] X Position
  285. * @param y [m] Y Position
  286. * @param z [m] Z Position
  287. * @param vx [m/s] X Speed
  288. * @param vy [m/s] Y Speed
  289. * @param vz [m/s] Z Speed
  290. * @param ax [m/s/s] X Acceleration
  291. * @param ay [m/s/s] Y Acceleration
  292. * @param az [m/s/s] Z Acceleration
  293. * @param covariance Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array.
  294. */
  295. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  296. static inline void mavlink_msg_local_position_ned_cov_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance)
  297. {
  298. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  299. char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN];
  300. _mav_put_uint64_t(buf, 0, time_usec);
  301. _mav_put_float(buf, 8, x);
  302. _mav_put_float(buf, 12, y);
  303. _mav_put_float(buf, 16, z);
  304. _mav_put_float(buf, 20, vx);
  305. _mav_put_float(buf, 24, vy);
  306. _mav_put_float(buf, 28, vz);
  307. _mav_put_float(buf, 32, ax);
  308. _mav_put_float(buf, 36, ay);
  309. _mav_put_float(buf, 40, az);
  310. _mav_put_uint8_t(buf, 224, estimator_type);
  311. _mav_put_float_array(buf, 44, covariance, 45);
  312. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  313. #else
  314. mavlink_local_position_ned_cov_t packet;
  315. packet.time_usec = time_usec;
  316. packet.x = x;
  317. packet.y = y;
  318. packet.z = z;
  319. packet.vx = vx;
  320. packet.vy = vy;
  321. packet.vz = vz;
  322. packet.ax = ax;
  323. packet.ay = ay;
  324. packet.az = az;
  325. packet.estimator_type = estimator_type;
  326. mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45);
  327. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  328. #endif
  329. }
  330. /**
  331. * @brief Send a local_position_ned_cov message
  332. * @param chan MAVLink channel to send the message
  333. * @param struct The MAVLink struct to serialize
  334. */
  335. static inline void mavlink_msg_local_position_ned_cov_send_struct(mavlink_channel_t chan, const mavlink_local_position_ned_cov_t* local_position_ned_cov)
  336. {
  337. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  338. mavlink_msg_local_position_ned_cov_send(chan, local_position_ned_cov->time_usec, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance);
  339. #else
  340. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)local_position_ned_cov, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  341. #endif
  342. }
  343. #if MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN
  344. /*
  345. This variant of _send() can be used to save stack space by re-using
  346. memory from the receive buffer. The caller provides a
  347. mavlink_message_t which is the size of a full mavlink message. This
  348. is usually the receive buffer for the channel, and allows a reply to an
  349. incoming message with minimum stack space usage.
  350. */
  351. static inline void mavlink_msg_local_position_ned_cov_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance)
  352. {
  353. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  354. char *buf = (char *)msgbuf;
  355. _mav_put_uint64_t(buf, 0, time_usec);
  356. _mav_put_float(buf, 8, x);
  357. _mav_put_float(buf, 12, y);
  358. _mav_put_float(buf, 16, z);
  359. _mav_put_float(buf, 20, vx);
  360. _mav_put_float(buf, 24, vy);
  361. _mav_put_float(buf, 28, vz);
  362. _mav_put_float(buf, 32, ax);
  363. _mav_put_float(buf, 36, ay);
  364. _mav_put_float(buf, 40, az);
  365. _mav_put_uint8_t(buf, 224, estimator_type);
  366. _mav_put_float_array(buf, 44, covariance, 45);
  367. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  368. #else
  369. mavlink_local_position_ned_cov_t *packet = (mavlink_local_position_ned_cov_t *)msgbuf;
  370. packet->time_usec = time_usec;
  371. packet->x = x;
  372. packet->y = y;
  373. packet->z = z;
  374. packet->vx = vx;
  375. packet->vy = vy;
  376. packet->vz = vz;
  377. packet->ax = ax;
  378. packet->ay = ay;
  379. packet->az = az;
  380. packet->estimator_type = estimator_type;
  381. mav_array_memcpy(packet->covariance, covariance, sizeof(float)*45);
  382. _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC);
  383. #endif
  384. }
  385. #endif
  386. #endif
  387. // MESSAGE LOCAL_POSITION_NED_COV UNPACKING
  388. /**
  389. * @brief Get field time_usec from local_position_ned_cov message
  390. *
  391. * @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.
  392. */
  393. static inline uint64_t mavlink_msg_local_position_ned_cov_get_time_usec(const mavlink_message_t* msg)
  394. {
  395. return _MAV_RETURN_uint64_t(msg, 0);
  396. }
  397. /**
  398. * @brief Get field estimator_type from local_position_ned_cov message
  399. *
  400. * @return Class id of the estimator this estimate originated from.
  401. */
  402. static inline uint8_t mavlink_msg_local_position_ned_cov_get_estimator_type(const mavlink_message_t* msg)
  403. {
  404. return _MAV_RETURN_uint8_t(msg, 224);
  405. }
  406. /**
  407. * @brief Get field x from local_position_ned_cov message
  408. *
  409. * @return [m] X Position
  410. */
  411. static inline float mavlink_msg_local_position_ned_cov_get_x(const mavlink_message_t* msg)
  412. {
  413. return _MAV_RETURN_float(msg, 8);
  414. }
  415. /**
  416. * @brief Get field y from local_position_ned_cov message
  417. *
  418. * @return [m] Y Position
  419. */
  420. static inline float mavlink_msg_local_position_ned_cov_get_y(const mavlink_message_t* msg)
  421. {
  422. return _MAV_RETURN_float(msg, 12);
  423. }
  424. /**
  425. * @brief Get field z from local_position_ned_cov message
  426. *
  427. * @return [m] Z Position
  428. */
  429. static inline float mavlink_msg_local_position_ned_cov_get_z(const mavlink_message_t* msg)
  430. {
  431. return _MAV_RETURN_float(msg, 16);
  432. }
  433. /**
  434. * @brief Get field vx from local_position_ned_cov message
  435. *
  436. * @return [m/s] X Speed
  437. */
  438. static inline float mavlink_msg_local_position_ned_cov_get_vx(const mavlink_message_t* msg)
  439. {
  440. return _MAV_RETURN_float(msg, 20);
  441. }
  442. /**
  443. * @brief Get field vy from local_position_ned_cov message
  444. *
  445. * @return [m/s] Y Speed
  446. */
  447. static inline float mavlink_msg_local_position_ned_cov_get_vy(const mavlink_message_t* msg)
  448. {
  449. return _MAV_RETURN_float(msg, 24);
  450. }
  451. /**
  452. * @brief Get field vz from local_position_ned_cov message
  453. *
  454. * @return [m/s] Z Speed
  455. */
  456. static inline float mavlink_msg_local_position_ned_cov_get_vz(const mavlink_message_t* msg)
  457. {
  458. return _MAV_RETURN_float(msg, 28);
  459. }
  460. /**
  461. * @brief Get field ax from local_position_ned_cov message
  462. *
  463. * @return [m/s/s] X Acceleration
  464. */
  465. static inline float mavlink_msg_local_position_ned_cov_get_ax(const mavlink_message_t* msg)
  466. {
  467. return _MAV_RETURN_float(msg, 32);
  468. }
  469. /**
  470. * @brief Get field ay from local_position_ned_cov message
  471. *
  472. * @return [m/s/s] Y Acceleration
  473. */
  474. static inline float mavlink_msg_local_position_ned_cov_get_ay(const mavlink_message_t* msg)
  475. {
  476. return _MAV_RETURN_float(msg, 36);
  477. }
  478. /**
  479. * @brief Get field az from local_position_ned_cov message
  480. *
  481. * @return [m/s/s] Z Acceleration
  482. */
  483. static inline float mavlink_msg_local_position_ned_cov_get_az(const mavlink_message_t* msg)
  484. {
  485. return _MAV_RETURN_float(msg, 40);
  486. }
  487. /**
  488. * @brief Get field covariance from local_position_ned_cov message
  489. *
  490. * @return Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array.
  491. */
  492. static inline uint16_t mavlink_msg_local_position_ned_cov_get_covariance(const mavlink_message_t* msg, float *covariance)
  493. {
  494. return _MAV_RETURN_float_array(msg, covariance, 45, 44);
  495. }
  496. /**
  497. * @brief Decode a local_position_ned_cov message into a struct
  498. *
  499. * @param msg The message to decode
  500. * @param local_position_ned_cov C-struct to decode the message contents into
  501. */
  502. static inline void mavlink_msg_local_position_ned_cov_decode(const mavlink_message_t* msg, mavlink_local_position_ned_cov_t* local_position_ned_cov)
  503. {
  504. #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
  505. local_position_ned_cov->time_usec = mavlink_msg_local_position_ned_cov_get_time_usec(msg);
  506. local_position_ned_cov->x = mavlink_msg_local_position_ned_cov_get_x(msg);
  507. local_position_ned_cov->y = mavlink_msg_local_position_ned_cov_get_y(msg);
  508. local_position_ned_cov->z = mavlink_msg_local_position_ned_cov_get_z(msg);
  509. local_position_ned_cov->vx = mavlink_msg_local_position_ned_cov_get_vx(msg);
  510. local_position_ned_cov->vy = mavlink_msg_local_position_ned_cov_get_vy(msg);
  511. local_position_ned_cov->vz = mavlink_msg_local_position_ned_cov_get_vz(msg);
  512. local_position_ned_cov->ax = mavlink_msg_local_position_ned_cov_get_ax(msg);
  513. local_position_ned_cov->ay = mavlink_msg_local_position_ned_cov_get_ay(msg);
  514. local_position_ned_cov->az = mavlink_msg_local_position_ned_cov_get_az(msg);
  515. mavlink_msg_local_position_ned_cov_get_covariance(msg, local_position_ned_cov->covariance);
  516. local_position_ned_cov->estimator_type = mavlink_msg_local_position_ned_cov_get_estimator_type(msg);
  517. #else
  518. uint8_t len = msg->len < MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN? msg->len : MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN;
  519. memset(local_position_ned_cov, 0, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN);
  520. memcpy(local_position_ned_cov, _MAV_PAYLOAD(msg), len);
  521. #endif
  522. }