lib/main/MAVLink/common/mavlink_msg_odometry.h

   1 #pragma once
   2 // MESSAGE ODOMETRY PACKING
   3
   4 #define MAVLINK_MSG_ID_ODOMETRY 331
   5
   6
   7 typedef struct __mavlink_odometry_t {
   8  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.*/
   9  float x; /*< [m] X Position*/
  10  float y; /*< [m] Y Position*/
  11  float z; /*< [m] Z Position*/
  12  float q[4]; /*<  Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)*/
  13  float vx; /*< [m/s] X linear speed*/
  14  float vy; /*< [m/s] Y linear speed*/
  15  float vz; /*< [m/s] Z linear speed*/
  16  float rollspeed; /*< [rad/s] Roll angular speed*/
  17  float pitchspeed; /*< [rad/s] Pitch angular speed*/
  18  float yawspeed; /*< [rad/s] Yaw angular speed*/
  19  float pose_covariance[21]; /*<  Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/
  20  float velocity_covariance[21]; /*<  Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/
  21  uint8_t frame_id; /*<  Coordinate frame of reference for the pose data.*/
  22  uint8_t child_frame_id; /*<  Coordinate frame of reference for the velocity in free space (twist) data.*/
  23  uint8_t reset_counter; /*<  Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/
  24  uint8_t estimator_type; /*<  Type of estimator that is providing the odometry.*/
  25 } mavlink_odometry_t;
  26
  27 #define MAVLINK_MSG_ID_ODOMETRY_LEN 232
  28 #define MAVLINK_MSG_ID_ODOMETRY_MIN_LEN 230
  29 #define MAVLINK_MSG_ID_331_LEN 232
  30 #define MAVLINK_MSG_ID_331_MIN_LEN 230
  31
  32 #define MAVLINK_MSG_ID_ODOMETRY_CRC 91
  33 #define MAVLINK_MSG_ID_331_CRC 91
  34
  35 #define MAVLINK_MSG_ODOMETRY_FIELD_Q_LEN 4
  36 #define MAVLINK_MSG_ODOMETRY_FIELD_POSE_COVARIANCE_LEN 21
  37 #define MAVLINK_MSG_ODOMETRY_FIELD_VELOCITY_COVARIANCE_LEN 21
  38
  39 #if MAVLINK_COMMAND_24BIT
  40 #define MAVLINK_MESSAGE_INFO_ODOMETRY { \
  41     331, \
  42     "ODOMETRY", \
  43     17, \
  44     {  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \
  45          { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \
  46          { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \
  47          { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, x) }, \
  48          { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, y) }, \
  49          { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, z) }, \
  50          { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 20, offsetof(mavlink_odometry_t, q) }, \
  51          { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_odometry_t, vx) }, \
  52          { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_odometry_t, vy) }, \
  53          { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_odometry_t, vz) }, \
  54          { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_odometry_t, rollspeed) }, \
  55          { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_odometry_t, pitchspeed) }, \
  56          { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_odometry_t, yawspeed) }, \
  57          { "pose_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 60, offsetof(mavlink_odometry_t, pose_covariance) }, \
  58          { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \
  59          { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \
  60          { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \
  61          } \
  62 }
  63 #else
  64 #define MAVLINK_MESSAGE_INFO_ODOMETRY { \
  65     "ODOMETRY", \
  66     17, \
  67     {  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \
  68          { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \
  69          { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \
  70          { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, x) }, \
  71          { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, y) }, \
  72          { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, z) }, \
  73          { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 20, offsetof(mavlink_odometry_t, q) }, \
  74          { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_odometry_t, vx) }, \
  75          { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_odometry_t, vy) }, \
  76          { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_odometry_t, vz) }, \
  77          { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_odometry_t, rollspeed) }, \
  78          { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_odometry_t, pitchspeed) }, \
  79          { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_odometry_t, yawspeed) }, \
  80          { "pose_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 60, offsetof(mavlink_odometry_t, pose_covariance) }, \
  81          { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \
  82          { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \
  83          { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \
  84          } \
  85 }
  86 #endif
  87
  88 /**
  89  * @brief Pack a odometry message
  90  * @param system_id ID of this system
  91  * @param component_id ID of this component (e.g. 200 for IMU)
  92  * @param msg The MAVLink message to compress the data into
  93  *
  94  * @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.
  95  * @param frame_id  Coordinate frame of reference for the pose data.
  96  * @param child_frame_id  Coordinate frame of reference for the velocity in free space (twist) data.
  97  * @param x [m] X Position
  98  * @param y [m] Y Position
  99  * @param z [m] Z Position
 100  * @param q  Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)
 101  * @param vx [m/s] X linear speed
 102  * @param vy [m/s] Y linear speed
 103  * @param vz [m/s] Z linear speed
 104  * @param rollspeed [rad/s] Roll angular speed
 105  * @param pitchspeed [rad/s] Pitch angular speed
 106  * @param yawspeed [rad/s] Yaw angular speed
 107  * @param pose_covariance  Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 108  * @param velocity_covariance  Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 109  * @param reset_counter  Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.
 110  * @param estimator_type  Type of estimator that is providing the odometry.
 111  * @return length of the message in bytes (excluding serial stream start sign)
 112  */
 113 static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
 114                                uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type)
 115 {
 116 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
 117     char buf[MAVLINK_MSG_ID_ODOMETRY_LEN];
 118     _mav_put_uint64_t(buf, 0, time_usec);
 119     _mav_put_float(buf, 8, x);
 120     _mav_put_float(buf, 12, y);
 121     _mav_put_float(buf, 16, z);
 122     _mav_put_float(buf, 36, vx);
 123     _mav_put_float(buf, 40, vy);
 124     _mav_put_float(buf, 44, vz);
 125     _mav_put_float(buf, 48, rollspeed);
 126     _mav_put_float(buf, 52, pitchspeed);
 127     _mav_put_float(buf, 56, yawspeed);
 128     _mav_put_uint8_t(buf, 228, frame_id);
 129     _mav_put_uint8_t(buf, 229, child_frame_id);
 130     _mav_put_uint8_t(buf, 230, reset_counter);
 131     _mav_put_uint8_t(buf, 231, estimator_type);
 132     _mav_put_float_array(buf, 20, q, 4);
 133     _mav_put_float_array(buf, 60, pose_covariance, 21);
 134     _mav_put_float_array(buf, 144, velocity_covariance, 21);
 135         memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN);
 136 #else
 137     mavlink_odometry_t packet;
 138     packet.time_usec = time_usec;
 139     packet.x = x;
 140     packet.y = y;
 141     packet.z = z;
 142     packet.vx = vx;
 143     packet.vy = vy;
 144     packet.vz = vz;
 145     packet.rollspeed = rollspeed;
 146     packet.pitchspeed = pitchspeed;
 147     packet.yawspeed = yawspeed;
 148     packet.frame_id = frame_id;
 149     packet.child_frame_id = child_frame_id;
 150     packet.reset_counter = reset_counter;
 151     packet.estimator_type = estimator_type;
 152     mav_array_memcpy(packet.q, q, sizeof(float)*4);
 153     mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21);
 154     mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21);
 155         memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN);
 156 #endif
 157
 158     msg->msgid = MAVLINK_MSG_ID_ODOMETRY;
 159     return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 160 }
 161
 162 /**
 163  * @brief Pack a odometry message on a channel
 164  * @param system_id ID of this system
 165  * @param component_id ID of this component (e.g. 200 for IMU)
 166  * @param chan The MAVLink channel this message will be sent over
 167  * @param msg The MAVLink message to compress the data into
 168  * @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.
 169  * @param frame_id  Coordinate frame of reference for the pose data.
 170  * @param child_frame_id  Coordinate frame of reference for the velocity in free space (twist) data.
 171  * @param x [m] X Position
 172  * @param y [m] Y Position
 173  * @param z [m] Z Position
 174  * @param q  Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)
 175  * @param vx [m/s] X linear speed
 176  * @param vy [m/s] Y linear speed
 177  * @param vz [m/s] Z linear speed
 178  * @param rollspeed [rad/s] Roll angular speed
 179  * @param pitchspeed [rad/s] Pitch angular speed
 180  * @param yawspeed [rad/s] Yaw angular speed
 181  * @param pose_covariance  Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 182  * @param velocity_covariance  Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 183  * @param reset_counter  Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.
 184  * @param estimator_type  Type of estimator that is providing the odometry.
 185  * @return length of the message in bytes (excluding serial stream start sign)
 186  */
 187 static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
 188                                mavlink_message_t* msg,
 189                                    uint64_t time_usec,uint8_t frame_id,uint8_t child_frame_id,float x,float y,float z,const float *q,float vx,float vy,float vz,float rollspeed,float pitchspeed,float yawspeed,const float *pose_covariance,const float *velocity_covariance,uint8_t reset_counter,uint8_t estimator_type)
 190 {
 191 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
 192     char buf[MAVLINK_MSG_ID_ODOMETRY_LEN];
 193     _mav_put_uint64_t(buf, 0, time_usec);
 194     _mav_put_float(buf, 8, x);
 195     _mav_put_float(buf, 12, y);
 196     _mav_put_float(buf, 16, z);
 197     _mav_put_float(buf, 36, vx);
 198     _mav_put_float(buf, 40, vy);
 199     _mav_put_float(buf, 44, vz);
 200     _mav_put_float(buf, 48, rollspeed);
 201     _mav_put_float(buf, 52, pitchspeed);
 202     _mav_put_float(buf, 56, yawspeed);
 203     _mav_put_uint8_t(buf, 228, frame_id);
 204     _mav_put_uint8_t(buf, 229, child_frame_id);
 205     _mav_put_uint8_t(buf, 230, reset_counter);
 206     _mav_put_uint8_t(buf, 231, estimator_type);
 207     _mav_put_float_array(buf, 20, q, 4);
 208     _mav_put_float_array(buf, 60, pose_covariance, 21);
 209     _mav_put_float_array(buf, 144, velocity_covariance, 21);
 210         memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN);
 211 #else
 212     mavlink_odometry_t packet;
 213     packet.time_usec = time_usec;
 214     packet.x = x;
 215     packet.y = y;
 216     packet.z = z;
 217     packet.vx = vx;
 218     packet.vy = vy;
 219     packet.vz = vz;
 220     packet.rollspeed = rollspeed;
 221     packet.pitchspeed = pitchspeed;
 222     packet.yawspeed = yawspeed;
 223     packet.frame_id = frame_id;
 224     packet.child_frame_id = child_frame_id;
 225     packet.reset_counter = reset_counter;
 226     packet.estimator_type = estimator_type;
 227     mav_array_memcpy(packet.q, q, sizeof(float)*4);
 228     mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21);
 229     mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21);
 230         memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN);
 231 #endif
 232
 233     msg->msgid = MAVLINK_MSG_ID_ODOMETRY;
 234     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 235 }
 236
 237 /**
 238  * @brief Encode a odometry struct
 239  *
 240  * @param system_id ID of this system
 241  * @param component_id ID of this component (e.g. 200 for IMU)
 242  * @param msg The MAVLink message to compress the data into
 243  * @param odometry C-struct to read the message contents from
 244  */
 245 static inline uint16_t mavlink_msg_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_odometry_t* odometry)
 246 {
 247     return mavlink_msg_odometry_pack(system_id, component_id, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type);
 248 }
 249
 250 /**
 251  * @brief Encode a odometry struct on a channel
 252  *
 253  * @param system_id ID of this system
 254  * @param component_id ID of this component (e.g. 200 for IMU)
 255  * @param chan The MAVLink channel this message will be sent over
 256  * @param msg The MAVLink message to compress the data into
 257  * @param odometry C-struct to read the message contents from
 258  */
 259 static inline uint16_t mavlink_msg_odometry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_odometry_t* odometry)
 260 {
 261     return mavlink_msg_odometry_pack_chan(system_id, component_id, chan, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type);
 262 }
 263
 264 /**
 265  * @brief Send a odometry message
 266  * @param chan MAVLink channel to send the message
 267  *
 268  * @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.
 269  * @param frame_id  Coordinate frame of reference for the pose data.
 270  * @param child_frame_id  Coordinate frame of reference for the velocity in free space (twist) data.
 271  * @param x [m] X Position
 272  * @param y [m] Y Position
 273  * @param z [m] Z Position
 274  * @param q  Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)
 275  * @param vx [m/s] X linear speed
 276  * @param vy [m/s] Y linear speed
 277  * @param vz [m/s] Z linear speed
 278  * @param rollspeed [rad/s] Roll angular speed
 279  * @param pitchspeed [rad/s] Pitch angular speed
 280  * @param yawspeed [rad/s] Yaw angular speed
 281  * @param pose_covariance  Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 282  * @param velocity_covariance  Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 283  * @param reset_counter  Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.
 284  * @param estimator_type  Type of estimator that is providing the odometry.
 285  */
 286 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
 287
 288 static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type)
 289 {
 290 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
 291     char buf[MAVLINK_MSG_ID_ODOMETRY_LEN];
 292     _mav_put_uint64_t(buf, 0, time_usec);
 293     _mav_put_float(buf, 8, x);
 294     _mav_put_float(buf, 12, y);
 295     _mav_put_float(buf, 16, z);
 296     _mav_put_float(buf, 36, vx);
 297     _mav_put_float(buf, 40, vy);
 298     _mav_put_float(buf, 44, vz);
 299     _mav_put_float(buf, 48, rollspeed);
 300     _mav_put_float(buf, 52, pitchspeed);
 301     _mav_put_float(buf, 56, yawspeed);
 302     _mav_put_uint8_t(buf, 228, frame_id);
 303     _mav_put_uint8_t(buf, 229, child_frame_id);
 304     _mav_put_uint8_t(buf, 230, reset_counter);
 305     _mav_put_uint8_t(buf, 231, estimator_type);
 306     _mav_put_float_array(buf, 20, q, 4);
 307     _mav_put_float_array(buf, 60, pose_covariance, 21);
 308     _mav_put_float_array(buf, 144, velocity_covariance, 21);
 309     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 310 #else
 311     mavlink_odometry_t packet;
 312     packet.time_usec = time_usec;
 313     packet.x = x;
 314     packet.y = y;
 315     packet.z = z;
 316     packet.vx = vx;
 317     packet.vy = vy;
 318     packet.vz = vz;
 319     packet.rollspeed = rollspeed;
 320     packet.pitchspeed = pitchspeed;
 321     packet.yawspeed = yawspeed;
 322     packet.frame_id = frame_id;
 323     packet.child_frame_id = child_frame_id;
 324     packet.reset_counter = reset_counter;
 325     packet.estimator_type = estimator_type;
 326     mav_array_memcpy(packet.q, q, sizeof(float)*4);
 327     mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21);
 328     mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21);
 329     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)&packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 330 #endif
 331 }
 332
 333 /**
 334  * @brief Send a odometry message
 335  * @param chan MAVLink channel to send the message
 336  * @param struct The MAVLink struct to serialize
 337  */
 338 static inline void mavlink_msg_odometry_send_struct(mavlink_channel_t chan, const mavlink_odometry_t* odometry)
 339 {
 340 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
 341     mavlink_msg_odometry_send(chan, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type);
 342 #else
 343     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)odometry, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 344 #endif
 345 }
 346
 347 #if MAVLINK_MSG_ID_ODOMETRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
 348 /*
 349   This varient of _send() can be used to save stack space by re-using
 350   memory from the receive buffer.  The caller provides a
 351   mavlink_message_t which is the size of a full mavlink message. This
 352   is usually the receive buffer for the channel, and allows a reply to an
 353   incoming message with minimum stack space usage.
 354  */
 355 static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type)
 356 {
 357 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
 358     char *buf = (char *)msgbuf;
 359     _mav_put_uint64_t(buf, 0, time_usec);
 360     _mav_put_float(buf, 8, x);
 361     _mav_put_float(buf, 12, y);
 362     _mav_put_float(buf, 16, z);
 363     _mav_put_float(buf, 36, vx);
 364     _mav_put_float(buf, 40, vy);
 365     _mav_put_float(buf, 44, vz);
 366     _mav_put_float(buf, 48, rollspeed);
 367     _mav_put_float(buf, 52, pitchspeed);
 368     _mav_put_float(buf, 56, yawspeed);
 369     _mav_put_uint8_t(buf, 228, frame_id);
 370     _mav_put_uint8_t(buf, 229, child_frame_id);
 371     _mav_put_uint8_t(buf, 230, reset_counter);
 372     _mav_put_uint8_t(buf, 231, estimator_type);
 373     _mav_put_float_array(buf, 20, q, 4);
 374     _mav_put_float_array(buf, 60, pose_covariance, 21);
 375     _mav_put_float_array(buf, 144, velocity_covariance, 21);
 376     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 377 #else
 378     mavlink_odometry_t *packet = (mavlink_odometry_t *)msgbuf;
 379     packet->time_usec = time_usec;
 380     packet->x = x;
 381     packet->y = y;
 382     packet->z = z;
 383     packet->vx = vx;
 384     packet->vy = vy;
 385     packet->vz = vz;
 386     packet->rollspeed = rollspeed;
 387     packet->pitchspeed = pitchspeed;
 388     packet->yawspeed = yawspeed;
 389     packet->frame_id = frame_id;
 390     packet->child_frame_id = child_frame_id;
 391     packet->reset_counter = reset_counter;
 392     packet->estimator_type = estimator_type;
 393     mav_array_memcpy(packet->q, q, sizeof(float)*4);
 394     mav_array_memcpy(packet->pose_covariance, pose_covariance, sizeof(float)*21);
 395     mav_array_memcpy(packet->velocity_covariance, velocity_covariance, sizeof(float)*21);
 396     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC);
 397 #endif
 398 }
 399 #endif
 400
 401 #endif
 402
 403 // MESSAGE ODOMETRY UNPACKING
 404
 405
 406 /**
 407  * @brief Get field time_usec from odometry message
 408  *
 409  * @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.
 410  */
 411 static inline uint64_t mavlink_msg_odometry_get_time_usec(const mavlink_message_t* msg)
 412 {
 413     return _MAV_RETURN_uint64_t(msg,  0);
 414 }
 415
 416 /**
 417  * @brief Get field frame_id from odometry message
 418  *
 419  * @return  Coordinate frame of reference for the pose data.
 420  */
 421 static inline uint8_t mavlink_msg_odometry_get_frame_id(const mavlink_message_t* msg)
 422 {
 423     return _MAV_RETURN_uint8_t(msg,  228);
 424 }
 425
 426 /**
 427  * @brief Get field child_frame_id from odometry message
 428  *
 429  * @return  Coordinate frame of reference for the velocity in free space (twist) data.
 430  */
 431 static inline uint8_t mavlink_msg_odometry_get_child_frame_id(const mavlink_message_t* msg)
 432 {
 433     return _MAV_RETURN_uint8_t(msg,  229);
 434 }
 435
 436 /**
 437  * @brief Get field x from odometry message
 438  *
 439  * @return [m] X Position
 440  */
 441 static inline float mavlink_msg_odometry_get_x(const mavlink_message_t* msg)
 442 {
 443     return _MAV_RETURN_float(msg,  8);
 444 }
 445
 446 /**
 447  * @brief Get field y from odometry message
 448  *
 449  * @return [m] Y Position
 450  */
 451 static inline float mavlink_msg_odometry_get_y(const mavlink_message_t* msg)
 452 {
 453     return _MAV_RETURN_float(msg,  12);
 454 }
 455
 456 /**
 457  * @brief Get field z from odometry message
 458  *
 459  * @return [m] Z Position
 460  */
 461 static inline float mavlink_msg_odometry_get_z(const mavlink_message_t* msg)
 462 {
 463     return _MAV_RETURN_float(msg,  16);
 464 }
 465
 466 /**
 467  * @brief Get field q from odometry message
 468  *
 469  * @return  Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)
 470  */
 471 static inline uint16_t mavlink_msg_odometry_get_q(const mavlink_message_t* msg, float *q)
 472 {
 473     return _MAV_RETURN_float_array(msg, q, 4,  20);
 474 }
 475
 476 /**
 477  * @brief Get field vx from odometry message
 478  *
 479  * @return [m/s] X linear speed
 480  */
 481 static inline float mavlink_msg_odometry_get_vx(const mavlink_message_t* msg)
 482 {
 483     return _MAV_RETURN_float(msg,  36);
 484 }
 485
 486 /**
 487  * @brief Get field vy from odometry message
 488  *
 489  * @return [m/s] Y linear speed
 490  */
 491 static inline float mavlink_msg_odometry_get_vy(const mavlink_message_t* msg)
 492 {
 493     return _MAV_RETURN_float(msg,  40);
 494 }
 495
 496 /**
 497  * @brief Get field vz from odometry message
 498  *
 499  * @return [m/s] Z linear speed
 500  */
 501 static inline float mavlink_msg_odometry_get_vz(const mavlink_message_t* msg)
 502 {
 503     return _MAV_RETURN_float(msg,  44);
 504 }
 505
 506 /**
 507  * @brief Get field rollspeed from odometry message
 508  *
 509  * @return [rad/s] Roll angular speed
 510  */
 511 static inline float mavlink_msg_odometry_get_rollspeed(const mavlink_message_t* msg)
 512 {
 513     return _MAV_RETURN_float(msg,  48);
 514 }
 515
 516 /**
 517  * @brief Get field pitchspeed from odometry message
 518  *
 519  * @return [rad/s] Pitch angular speed
 520  */
 521 static inline float mavlink_msg_odometry_get_pitchspeed(const mavlink_message_t* msg)
 522 {
 523     return _MAV_RETURN_float(msg,  52);
 524 }
 525
 526 /**
 527  * @brief Get field yawspeed from odometry message
 528  *
 529  * @return [rad/s] Yaw angular speed
 530  */
 531 static inline float mavlink_msg_odometry_get_yawspeed(const mavlink_message_t* msg)
 532 {
 533     return _MAV_RETURN_float(msg,  56);
 534 }
 535
 536 /**
 537  * @brief Get field pose_covariance from odometry message
 538  *
 539  * @return  Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 540  */
 541 static inline uint16_t mavlink_msg_odometry_get_pose_covariance(const mavlink_message_t* msg, float *pose_covariance)
 542 {
 543     return _MAV_RETURN_float_array(msg, pose_covariance, 21,  60);
 544 }
 545
 546 /**
 547  * @brief Get field velocity_covariance from odometry message
 548  *
 549  * @return  Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.
 550  */
 551 static inline uint16_t mavlink_msg_odometry_get_velocity_covariance(const mavlink_message_t* msg, float *velocity_covariance)
 552 {
 553     return _MAV_RETURN_float_array(msg, velocity_covariance, 21,  144);
 554 }
 555
 556 /**
 557  * @brief Get field reset_counter from odometry message
 558  *
 559  * @return  Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.
 560  */
 561 static inline uint8_t mavlink_msg_odometry_get_reset_counter(const mavlink_message_t* msg)
 562 {
 563     return _MAV_RETURN_uint8_t(msg,  230);
 564 }
 565
 566 /**
 567  * @brief Get field estimator_type from odometry message
 568  *
 569  * @return  Type of estimator that is providing the odometry.
 570  */
 571 static inline uint8_t mavlink_msg_odometry_get_estimator_type(const mavlink_message_t* msg)
 572 {
 573     return _MAV_RETURN_uint8_t(msg,  231);
 574 }
 575
 576 /**
 577  * @brief Decode a odometry message into a struct
 578  *
 579  * @param msg The message to decode
 580  * @param odometry C-struct to decode the message contents into
 581  */
 582 static inline void mavlink_msg_odometry_decode(const mavlink_message_t* msg, mavlink_odometry_t* odometry)
 583 {
 584 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
 585     odometry->time_usec = mavlink_msg_odometry_get_time_usec(msg);
 586     odometry->x = mavlink_msg_odometry_get_x(msg);
 587     odometry->y = mavlink_msg_odometry_get_y(msg);
 588     odometry->z = mavlink_msg_odometry_get_z(msg);
 589     mavlink_msg_odometry_get_q(msg, odometry->q);
 590     odometry->vx = mavlink_msg_odometry_get_vx(msg);
 591     odometry->vy = mavlink_msg_odometry_get_vy(msg);
 592     odometry->vz = mavlink_msg_odometry_get_vz(msg);
 593     odometry->rollspeed = mavlink_msg_odometry_get_rollspeed(msg);
 594     odometry->pitchspeed = mavlink_msg_odometry_get_pitchspeed(msg);
 595     odometry->yawspeed = mavlink_msg_odometry_get_yawspeed(msg);
 596     mavlink_msg_odometry_get_pose_covariance(msg, odometry->pose_covariance);
 597     mavlink_msg_odometry_get_velocity_covariance(msg, odometry->velocity_covariance);
 598     odometry->frame_id = mavlink_msg_odometry_get_frame_id(msg);
 599     odometry->child_frame_id = mavlink_msg_odometry_get_child_frame_id(msg);
 600     odometry->reset_counter = mavlink_msg_odometry_get_reset_counter(msg);
 601     odometry->estimator_type = mavlink_msg_odometry_get_estimator_type(msg);
 602 #else
 603         uint8_t len = msg->len < MAVLINK_MSG_ID_ODOMETRY_LEN? msg->len : MAVLINK_MSG_ID_ODOMETRY_LEN;
 604         memset(odometry, 0, MAVLINK_MSG_ID_ODOMETRY_LEN);
 605     memcpy(odometry, _MAV_PAYLOAD(msg), len);
 606 #endif
 607 }