mavlink: protocol.h Source File

Go to the documentation of this file.
00001 #ifndef  _MAVLINK_PROTOCOL_H_
00002 #define  _MAVLINK_PROTOCOL_H_
00003 
00004 #include "string.h"
00005 #include "mavlink_types.h"
00006 
00007 /* 
00008    If you want MAVLink on a system that is native big-endian,
00009    you need to define NATIVE_BIG_ENDIAN
00010 */
00011 #ifdef NATIVE_BIG_ENDIAN
00012 # define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
00013 #else
00014 # define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
00015 #endif
00016 
00017 #ifndef MAVLINK_STACK_BUFFER
00018 #define MAVLINK_STACK_BUFFER 0
00019 #endif
00020 
00021 #ifndef MAVLINK_AVOID_GCC_STACK_BUG
00022 # define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
00023 #endif
00024 
00025 #ifndef MAVLINK_ASSERT
00026 #define MAVLINK_ASSERT(x)
00027 #endif
00028 
00029 #ifndef MAVLINK_START_UART_SEND
00030 #define MAVLINK_START_UART_SEND(chan, length)
00031 #endif
00032 
00033 #ifndef MAVLINK_END_UART_SEND
00034 #define MAVLINK_END_UART_SEND(chan, length)
00035 #endif
00036 
00037 #ifdef MAVLINK_SEPARATE_HELPERS
00038 #define MAVLINK_HELPER
00039 #else
00040 #define MAVLINK_HELPER static inline
00041 #include "mavlink_helpers.h"
00042 #endif // MAVLINK_SEPARATE_HELPERS
00043 
00044 /* always include the prototypes to ensure we don't get out of sync */
00045 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
00046 #if MAVLINK_CRC_EXTRA
00047 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00048                                                       uint8_t chan, uint8_t length, uint8_t crc_extra);
00049 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00050                                                  uint8_t length, uint8_t crc_extra);
00051 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00052 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
00053                                                     uint8_t length, uint8_t crc_extra);
00054 #endif
00055 #else
00056 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00057                                                       uint8_t chan, uint8_t length);
00058 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
00059                                                  uint8_t length);
00060 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
00061 #endif // MAVLINK_CRC_EXTRA
00062 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
00063 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
00064 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
00065 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
00066 MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, 
00067                                                uint8_t* r_bit_index, uint8_t* buffer);
00068 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00069 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
00070 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg);
00071 #endif
00072 
00076 static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
00077 {
00078         return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
00079 }
00080 
00081 #if MAVLINK_NEED_BYTE_SWAP
00082 static inline void byte_swap_2(char *dst, const char *src)
00083 {
00084         dst[0] = src[1];
00085         dst[1] = src[0];
00086 }
00087 static inline void byte_swap_4(char *dst, const char *src)
00088 {
00089         dst[0] = src[3];
00090         dst[1] = src[2];
00091         dst[2] = src[1];
00092         dst[3] = src[0];
00093 }
00094 static inline void byte_swap_8(char *dst, const char *src)
00095 {
00096         dst[0] = src[7];
00097         dst[1] = src[6];
00098         dst[2] = src[5];
00099         dst[3] = src[4];
00100         dst[4] = src[3];
00101         dst[5] = src[2];
00102         dst[6] = src[1];
00103         dst[7] = src[0];
00104 }
00105 #elif !MAVLINK_ALIGNED_FIELDS
00106 static inline void byte_copy_2(char *dst, const char *src)
00107 {
00108         dst[0] = src[0];
00109         dst[1] = src[1];
00110 }
00111 static inline void byte_copy_4(char *dst, const char *src)
00112 {
00113         dst[0] = src[0];
00114         dst[1] = src[1];
00115         dst[2] = src[2];
00116         dst[3] = src[3];
00117 }
00118 static inline void byte_copy_8(char *dst, const char *src)
00119 {
00120         memcpy(dst, src, 8);
00121 }
00122 #endif
00123 
00124 #define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
00125 #define _mav_put_int8_t(buf, wire_offset, b)  buf[wire_offset] = (int8_t)b
00126 #define _mav_put_char(buf, wire_offset, b)    buf[wire_offset] = b
00127 
00128 #if MAVLINK_NEED_BYTE_SWAP
00129 #define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
00130 #define _mav_put_int16_t(buf, wire_offset, b)  byte_swap_2(&buf[wire_offset], (const char *)&b)
00131 #define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
00132 #define _mav_put_int32_t(buf, wire_offset, b)  byte_swap_4(&buf[wire_offset], (const char *)&b)
00133 #define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
00134 #define _mav_put_int64_t(buf, wire_offset, b)  byte_swap_8(&buf[wire_offset], (const char *)&b)
00135 #define _mav_put_float(buf, wire_offset, b)    byte_swap_4(&buf[wire_offset], (const char *)&b)
00136 #define _mav_put_double(buf, wire_offset, b)   byte_swap_8(&buf[wire_offset], (const char *)&b)
00137 #elif !MAVLINK_ALIGNED_FIELDS
00138 #define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
00139 #define _mav_put_int16_t(buf, wire_offset, b)  byte_copy_2(&buf[wire_offset], (const char *)&b)
00140 #define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
00141 #define _mav_put_int32_t(buf, wire_offset, b)  byte_copy_4(&buf[wire_offset], (const char *)&b)
00142 #define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
00143 #define _mav_put_int64_t(buf, wire_offset, b)  byte_copy_8(&buf[wire_offset], (const char *)&b)
00144 #define _mav_put_float(buf, wire_offset, b)    byte_copy_4(&buf[wire_offset], (const char *)&b)
00145 #define _mav_put_double(buf, wire_offset, b)   byte_copy_8(&buf[wire_offset], (const char *)&b)
00146 #else
00147 #define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
00148 #define _mav_put_int16_t(buf, wire_offset, b)  *(int16_t *)&buf[wire_offset] = b
00149 #define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
00150 #define _mav_put_int32_t(buf, wire_offset, b)  *(int32_t *)&buf[wire_offset] = b
00151 #define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
00152 #define _mav_put_int64_t(buf, wire_offset, b)  *(int64_t *)&buf[wire_offset] = b
00153 #define _mav_put_float(buf, wire_offset, b)    *(float *)&buf[wire_offset] = b
00154 #define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset] = b
00155 #endif
00156 
00157 /*
00158   like memcpy(), but if src is NULL, do a memset to zero
00159 */
00160 static void mav_array_memcpy(void *dest, const void *src, size_t n)
00161 {
00162         if (src == NULL) {
00163                 memset(dest, 0, n);
00164         } else {
00165                 memcpy(dest, src, n);
00166         }
00167 }
00168 
00169 /*
00170  * Place a char array into a buffer
00171  */
00172 static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
00173 {
00174         mav_array_memcpy(&buf[wire_offset], b, array_length);
00175 
00176 }
00177 
00178 /*
00179  * Place a uint8_t array into a buffer
00180  */
00181 static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
00182 {
00183         mav_array_memcpy(&buf[wire_offset], b, array_length);
00184 
00185 }
00186 
00187 /*
00188  * Place a int8_t array into a buffer
00189  */
00190 static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
00191 {
00192         mav_array_memcpy(&buf[wire_offset], b, array_length);
00193 
00194 }
00195 
00196 #if MAVLINK_NEED_BYTE_SWAP
00197 #define _MAV_PUT_ARRAY(TYPE, V) \
00198 static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
00199 { \
00200         if (b == NULL) { \
00201                 memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
00202         } else { \
00203                 uint16_t i; \
00204                 for (i=0; i<array_length; i++) { \
00205                         _mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
00206                 } \
00207         } \
00208 }
00209 #else
00210 #define _MAV_PUT_ARRAY(TYPE, V)                                 \
00211 static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
00212 { \
00213         mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
00214 }
00215 #endif
00216 
00217 _MAV_PUT_ARRAY(uint16_t, u16)
00218 _MAV_PUT_ARRAY(uint32_t, u32)
00219 _MAV_PUT_ARRAY(uint64_t, u64)
00220 _MAV_PUT_ARRAY(int16_t,  i16)
00221 _MAV_PUT_ARRAY(int32_t,  i32)
00222 _MAV_PUT_ARRAY(int64_t,  i64)
00223 _MAV_PUT_ARRAY(float,    f)
00224 _MAV_PUT_ARRAY(double,   d)
00225 
00226 #define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
00227 #define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
00228 #define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
00229 
00230 #if MAVLINK_NEED_BYTE_SWAP
00231 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
00232 static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
00233 { TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
00234 
00235 _MAV_MSG_RETURN_TYPE(uint16_t, 2)
00236 _MAV_MSG_RETURN_TYPE(int16_t,  2)
00237 _MAV_MSG_RETURN_TYPE(uint32_t, 4)
00238 _MAV_MSG_RETURN_TYPE(int32_t,  4)
00239 _MAV_MSG_RETURN_TYPE(uint64_t, 8)
00240 _MAV_MSG_RETURN_TYPE(int64_t,  8)
00241 _MAV_MSG_RETURN_TYPE(float,    4)
00242 _MAV_MSG_RETURN_TYPE(double,   8)
00243 
00244 #elif !MAVLINK_ALIGNED_FIELDS
00245 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
00246 static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
00247 { TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
00248 
00249 _MAV_MSG_RETURN_TYPE(uint16_t, 2)
00250 _MAV_MSG_RETURN_TYPE(int16_t,  2)
00251 _MAV_MSG_RETURN_TYPE(uint32_t, 4)
00252 _MAV_MSG_RETURN_TYPE(int32_t,  4)
00253 _MAV_MSG_RETURN_TYPE(uint64_t, 8)
00254 _MAV_MSG_RETURN_TYPE(int64_t,  8)
00255 _MAV_MSG_RETURN_TYPE(float,    4)
00256 _MAV_MSG_RETURN_TYPE(double,   8)
00257 #else // nicely aligned, no swap
00258 #define _MAV_MSG_RETURN_TYPE(TYPE) \
00259 static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
00260 { return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
00261 
00262 _MAV_MSG_RETURN_TYPE(uint16_t)
00263 _MAV_MSG_RETURN_TYPE(int16_t)
00264 _MAV_MSG_RETURN_TYPE(uint32_t)
00265 _MAV_MSG_RETURN_TYPE(int32_t)
00266 _MAV_MSG_RETURN_TYPE(uint64_t)
00267 _MAV_MSG_RETURN_TYPE(int64_t)
00268 _MAV_MSG_RETURN_TYPE(float)
00269 _MAV_MSG_RETURN_TYPE(double)
00270 #endif // MAVLINK_NEED_BYTE_SWAP
00271 
00272 static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, 
00273                                                      uint8_t array_length, uint8_t wire_offset)
00274 {
00275         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
00276         return array_length;
00277 }
00278 
00279 static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, 
00280                                                         uint8_t array_length, uint8_t wire_offset)
00281 {
00282         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
00283         return array_length;
00284 }
00285 
00286 static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, 
00287                                                        uint8_t array_length, uint8_t wire_offset)
00288 {
00289         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
00290         return array_length;
00291 }
00292 
00293 #if MAVLINK_NEED_BYTE_SWAP
00294 #define _MAV_RETURN_ARRAY(TYPE, V) \
00295 static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
00296                                                          uint8_t array_length, uint8_t wire_offset) \
00297 { \
00298         uint16_t i; \
00299         for (i=0; i<array_length; i++) { \
00300                 value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
00301         } \
00302         return array_length*sizeof(value[0]); \
00303 }
00304 #else
00305 #define _MAV_RETURN_ARRAY(TYPE, V)                                      \
00306 static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
00307                                                          uint8_t array_length, uint8_t wire_offset) \
00308 { \
00309         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
00310         return array_length*sizeof(TYPE); \
00311 }
00312 #endif
00313 
00314 _MAV_RETURN_ARRAY(uint16_t, u16)
00315 _MAV_RETURN_ARRAY(uint32_t, u32)
00316 _MAV_RETURN_ARRAY(uint64_t, u64)
00317 _MAV_RETURN_ARRAY(int16_t,  i16)
00318 _MAV_RETURN_ARRAY(int32_t,  i32)
00319 _MAV_RETURN_ARRAY(int64_t,  i64)
00320 _MAV_RETURN_ARRAY(float,    f)
00321 _MAV_RETURN_ARRAY(double,   d)
00322 
00323 #endif // _MAVLINK_PROTOCOL_H_