Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
mavlink_msg_visual_odometry.h
00001 // MESSAGE VISUAL_ODOMETRY PACKING 00002 00003 #define MAVLINK_MSG_ID_VISUAL_ODOMETRY 180 00004 00005 typedef struct __mavlink_visual_odometry_t 00006 { 00007 uint64_t frame1_time_us; ///< Time at which frame 1 was captured (in microseconds since unix epoch) 00008 uint64_t frame2_time_us; ///< Time at which frame 2 was captured (in microseconds since unix epoch) 00009 float x; ///< Relative X position 00010 float y; ///< Relative Y position 00011 float z; ///< Relative Z position 00012 float roll; ///< Relative roll angle in rad 00013 float pitch; ///< Relative pitch angle in rad 00014 float yaw; ///< Relative yaw angle in rad 00015 00016 } mavlink_visual_odometry_t; 00017 00018 00019 00020 /** 00021 * @brief Pack a visual_odometry message 00022 * @param system_id ID of this system 00023 * @param component_id ID of this component (e.g. 200 for IMU) 00024 * @param msg The MAVLink message to compress the data into 00025 * 00026 * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch) 00027 * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch) 00028 * @param x Relative X position 00029 * @param y Relative Y position 00030 * @param z Relative Z position 00031 * @param roll Relative roll angle in rad 00032 * @param pitch Relative pitch angle in rad 00033 * @param yaw Relative yaw angle in rad 00034 * @return length of the message in bytes (excluding serial stream start sign) 00035 */ 00036 static inline uint16_t mavlink_msg_visual_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw) 00037 { 00038 uint16_t i = 0; 00039 msg->msgid = MAVLINK_MSG_ID_VISUAL_ODOMETRY; 00040 00041 i += put_uint64_t_by_index(frame1_time_us, i, msg->payload); // Time at which frame 1 was captured (in microseconds since unix epoch) 00042 i += put_uint64_t_by_index(frame2_time_us, i, msg->payload); // Time at which frame 2 was captured (in microseconds since unix epoch) 00043 i += put_float_by_index(x, i, msg->payload); // Relative X position 00044 i += put_float_by_index(y, i, msg->payload); // Relative Y position 00045 i += put_float_by_index(z, i, msg->payload); // Relative Z position 00046 i += put_float_by_index(roll, i, msg->payload); // Relative roll angle in rad 00047 i += put_float_by_index(pitch, i, msg->payload); // Relative pitch angle in rad 00048 i += put_float_by_index(yaw, i, msg->payload); // Relative yaw angle in rad 00049 00050 return mavlink_finalize_message(msg, system_id, component_id, i); 00051 } 00052 00053 /** 00054 * @brief Pack a visual_odometry message 00055 * @param system_id ID of this system 00056 * @param component_id ID of this component (e.g. 200 for IMU) 00057 * @param chan The MAVLink channel this message was sent over 00058 * @param msg The MAVLink message to compress the data into 00059 * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch) 00060 * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch) 00061 * @param x Relative X position 00062 * @param y Relative Y position 00063 * @param z Relative Z position 00064 * @param roll Relative roll angle in rad 00065 * @param pitch Relative pitch angle in rad 00066 * @param yaw Relative yaw angle in rad 00067 * @return length of the message in bytes (excluding serial stream start sign) 00068 */ 00069 static inline uint16_t mavlink_msg_visual_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw) 00070 { 00071 uint16_t i = 0; 00072 msg->msgid = MAVLINK_MSG_ID_VISUAL_ODOMETRY; 00073 00074 i += put_uint64_t_by_index(frame1_time_us, i, msg->payload); // Time at which frame 1 was captured (in microseconds since unix epoch) 00075 i += put_uint64_t_by_index(frame2_time_us, i, msg->payload); // Time at which frame 2 was captured (in microseconds since unix epoch) 00076 i += put_float_by_index(x, i, msg->payload); // Relative X position 00077 i += put_float_by_index(y, i, msg->payload); // Relative Y position 00078 i += put_float_by_index(z, i, msg->payload); // Relative Z position 00079 i += put_float_by_index(roll, i, msg->payload); // Relative roll angle in rad 00080 i += put_float_by_index(pitch, i, msg->payload); // Relative pitch angle in rad 00081 i += put_float_by_index(yaw, i, msg->payload); // Relative yaw angle in rad 00082 00083 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); 00084 } 00085 00086 /** 00087 * @brief Encode a visual_odometry struct into a message 00088 * 00089 * @param system_id ID of this system 00090 * @param component_id ID of this component (e.g. 200 for IMU) 00091 * @param msg The MAVLink message to compress the data into 00092 * @param visual_odometry C-struct to read the message contents from 00093 */ 00094 static inline uint16_t mavlink_msg_visual_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_visual_odometry_t* visual_odometry) 00095 { 00096 return mavlink_msg_visual_odometry_pack(system_id, component_id, msg, visual_odometry->frame1_time_us, visual_odometry->frame2_time_us, visual_odometry->x, visual_odometry->y, visual_odometry->z, visual_odometry->roll, visual_odometry->pitch, visual_odometry->yaw); 00097 } 00098 00099 /** 00100 * @brief Send a visual_odometry message 00101 * @param chan MAVLink channel to send the message 00102 * 00103 * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch) 00104 * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch) 00105 * @param x Relative X position 00106 * @param y Relative Y position 00107 * @param z Relative Z position 00108 * @param roll Relative roll angle in rad 00109 * @param pitch Relative pitch angle in rad 00110 * @param yaw Relative yaw angle in rad 00111 */ 00112 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 00113 00114 static inline void mavlink_msg_visual_odometry_send(mavlink_channel_t chan, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw) 00115 { 00116 mavlink_message_t msg; 00117 mavlink_msg_visual_odometry_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame1_time_us, frame2_time_us, x, y, z, roll, pitch, yaw); 00118 mavlink_send_uart(chan, &msg); 00119 } 00120 00121 #endif 00122 // MESSAGE VISUAL_ODOMETRY UNPACKING 00123 00124 /** 00125 * @brief Get field frame1_time_us from visual_odometry message 00126 * 00127 * @return Time at which frame 1 was captured (in microseconds since unix epoch) 00128 */ 00129 static inline uint64_t mavlink_msg_visual_odometry_get_frame1_time_us(const mavlink_message_t* msg) 00130 { 00131 generic_64bit r; 00132 r.b[7] = (msg->payload)[0]; 00133 r.b[6] = (msg->payload)[1]; 00134 r.b[5] = (msg->payload)[2]; 00135 r.b[4] = (msg->payload)[3]; 00136 r.b[3] = (msg->payload)[4]; 00137 r.b[2] = (msg->payload)[5]; 00138 r.b[1] = (msg->payload)[6]; 00139 r.b[0] = (msg->payload)[7]; 00140 return (uint64_t)r.ll; 00141 } 00142 00143 /** 00144 * @brief Get field frame2_time_us from visual_odometry message 00145 * 00146 * @return Time at which frame 2 was captured (in microseconds since unix epoch) 00147 */ 00148 static inline uint64_t mavlink_msg_visual_odometry_get_frame2_time_us(const mavlink_message_t* msg) 00149 { 00150 generic_64bit r; 00151 r.b[7] = (msg->payload+sizeof(uint64_t))[0]; 00152 r.b[6] = (msg->payload+sizeof(uint64_t))[1]; 00153 r.b[5] = (msg->payload+sizeof(uint64_t))[2]; 00154 r.b[4] = (msg->payload+sizeof(uint64_t))[3]; 00155 r.b[3] = (msg->payload+sizeof(uint64_t))[4]; 00156 r.b[2] = (msg->payload+sizeof(uint64_t))[5]; 00157 r.b[1] = (msg->payload+sizeof(uint64_t))[6]; 00158 r.b[0] = (msg->payload+sizeof(uint64_t))[7]; 00159 return (uint64_t)r.ll; 00160 } 00161 00162 /** 00163 * @brief Get field x from visual_odometry message 00164 * 00165 * @return Relative X position 00166 */ 00167 static inline float mavlink_msg_visual_odometry_get_x(const mavlink_message_t* msg) 00168 { 00169 generic_32bit r; 00170 r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[0]; 00171 r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[1]; 00172 r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[2]; 00173 r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[3]; 00174 return (float)r.f; 00175 } 00176 00177 /** 00178 * @brief Get field y from visual_odometry message 00179 * 00180 * @return Relative Y position 00181 */ 00182 static inline float mavlink_msg_visual_odometry_get_y(const mavlink_message_t* msg) 00183 { 00184 generic_32bit r; 00185 r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[0]; 00186 r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[1]; 00187 r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[2]; 00188 r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[3]; 00189 return (float)r.f; 00190 } 00191 00192 /** 00193 * @brief Get field z from visual_odometry message 00194 * 00195 * @return Relative Z position 00196 */ 00197 static inline float mavlink_msg_visual_odometry_get_z(const mavlink_message_t* msg) 00198 { 00199 generic_32bit r; 00200 r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; 00201 r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; 00202 r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; 00203 r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; 00204 return (float)r.f; 00205 } 00206 00207 /** 00208 * @brief Get field roll from visual_odometry message 00209 * 00210 * @return Relative roll angle in rad 00211 */ 00212 static inline float mavlink_msg_visual_odometry_get_roll(const mavlink_message_t* msg) 00213 { 00214 generic_32bit r; 00215 r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; 00216 r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; 00217 r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; 00218 r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; 00219 return (float)r.f; 00220 } 00221 00222 /** 00223 * @brief Get field pitch from visual_odometry message 00224 * 00225 * @return Relative pitch angle in rad 00226 */ 00227 static inline float mavlink_msg_visual_odometry_get_pitch(const mavlink_message_t* msg) 00228 { 00229 generic_32bit r; 00230 r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; 00231 r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; 00232 r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; 00233 r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; 00234 return (float)r.f; 00235 } 00236 00237 /** 00238 * @brief Get field yaw from visual_odometry message 00239 * 00240 * @return Relative yaw angle in rad 00241 */ 00242 static inline float mavlink_msg_visual_odometry_get_yaw(const mavlink_message_t* msg) 00243 { 00244 generic_32bit r; 00245 r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; 00246 r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; 00247 r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; 00248 r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; 00249 return (float)r.f; 00250 } 00251 00252 /** 00253 * @brief Decode a visual_odometry message into a struct 00254 * 00255 * @param msg The message to decode 00256 * @param visual_odometry C-struct to decode the message contents into 00257 */ 00258 static inline void mavlink_msg_visual_odometry_decode(const mavlink_message_t* msg, mavlink_visual_odometry_t* visual_odometry) 00259 { 00260 visual_odometry->frame1_time_us = mavlink_msg_visual_odometry_get_frame1_time_us(msg); 00261 visual_odometry->frame2_time_us = mavlink_msg_visual_odometry_get_frame2_time_us(msg); 00262 visual_odometry->x = mavlink_msg_visual_odometry_get_x(msg); 00263 visual_odometry->y = mavlink_msg_visual_odometry_get_y(msg); 00264 visual_odometry->z = mavlink_msg_visual_odometry_get_z(msg); 00265 visual_odometry->roll = mavlink_msg_visual_odometry_get_roll(msg); 00266 visual_odometry->pitch = mavlink_msg_visual_odometry_get_pitch(msg); 00267 visual_odometry->yaw = mavlink_msg_visual_odometry_get_yaw(msg); 00268 }
Generated on Tue Jul 12 2022 14:09:27 by 1.7.2