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 /* option to provide alternative implementation of mavlink_helpers.h */
00038 #ifdef MAVLINK_SEPARATE_HELPERS
00039 
00040     #define MAVLINK_HELPER
00041 
00042     /* decls in sync with those in mavlink_helpers.h */
00043     #ifndef MAVLINK_GET_CHANNEL_STATUS
00044     MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
00045     #endif
00046     MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
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 min_length, 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 min_length, 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, uint32_t msgid, const char *packet,
00053                                                         uint8_t min_length, uint8_t length, uint8_t crc_extra);
00054     #endif
00055     MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
00056     MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
00057     MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
00058     MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
00059                                                      mavlink_status_t* status,
00060                                                      uint8_t c, 
00061                                                      mavlink_message_t* r_message, 
00062                                                      mavlink_status_t* r_mavlink_status);
00063     MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
00064     MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
00065     MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index,
00066                                uint8_t* r_bit_index, uint8_t* buffer);
00067     MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid);
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 
00073 #else
00074 
00075     #define MAVLINK_HELPER static inline
00076     #include "mavlink_helpers.h"
00077 
00078 #endif // MAVLINK_SEPARATE_HELPERS
00079 
00080 
00084 static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
00085 {
00086         if (msg->magic == MAVLINK_STX_MAVLINK1) {
00087                 return msg->len + MAVLINK_CORE_HEADER_MAVLINK1_LEN+1 + 2;
00088         }
00089         uint16_t signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
00090         return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES + signature_len;
00091 }
00092 
00093 #if MAVLINK_NEED_BYTE_SWAP
00094 static inline void byte_swap_2(char *dst, const char *src)
00095 {
00096         dst[0] = src[1];
00097         dst[1] = src[0];
00098 }
00099 static inline void byte_swap_4(char *dst, const char *src)
00100 {
00101         dst[0] = src[3];
00102         dst[1] = src[2];
00103         dst[2] = src[1];
00104         dst[3] = src[0];
00105 }
00106 static inline void byte_swap_8(char *dst, const char *src)
00107 {
00108         dst[0] = src[7];
00109         dst[1] = src[6];
00110         dst[2] = src[5];
00111         dst[3] = src[4];
00112         dst[4] = src[3];
00113         dst[5] = src[2];
00114         dst[6] = src[1];
00115         dst[7] = src[0];
00116 }
00117 #elif !MAVLINK_ALIGNED_FIELDS
00118 static inline void byte_copy_2(char *dst, const char *src)
00119 {
00120         dst[0] = src[0];
00121         dst[1] = src[1];
00122 }
00123 static inline void byte_copy_4(char *dst, const char *src)
00124 {
00125         dst[0] = src[0];
00126         dst[1] = src[1];
00127         dst[2] = src[2];
00128         dst[3] = src[3];
00129 }
00130 static inline void byte_copy_8(char *dst, const char *src)
00131 {
00132         memcpy(dst, src, 8);
00133 }
00134 #endif
00135 
00136 #define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
00137 #define _mav_put_int8_t(buf, wire_offset, b)  buf[wire_offset] = (int8_t)b
00138 #define _mav_put_char(buf, wire_offset, b)    buf[wire_offset] = b
00139 
00140 #if MAVLINK_NEED_BYTE_SWAP
00141 #define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
00142 #define _mav_put_int16_t(buf, wire_offset, b)  byte_swap_2(&buf[wire_offset], (const char *)&b)
00143 #define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
00144 #define _mav_put_int32_t(buf, wire_offset, b)  byte_swap_4(&buf[wire_offset], (const char *)&b)
00145 #define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
00146 #define _mav_put_int64_t(buf, wire_offset, b)  byte_swap_8(&buf[wire_offset], (const char *)&b)
00147 #define _mav_put_float(buf, wire_offset, b)    byte_swap_4(&buf[wire_offset], (const char *)&b)
00148 #define _mav_put_double(buf, wire_offset, b)   byte_swap_8(&buf[wire_offset], (const char *)&b)
00149 #elif !MAVLINK_ALIGNED_FIELDS
00150 #define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
00151 #define _mav_put_int16_t(buf, wire_offset, b)  byte_copy_2(&buf[wire_offset], (const char *)&b)
00152 #define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
00153 #define _mav_put_int32_t(buf, wire_offset, b)  byte_copy_4(&buf[wire_offset], (const char *)&b)
00154 #define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
00155 #define _mav_put_int64_t(buf, wire_offset, b)  byte_copy_8(&buf[wire_offset], (const char *)&b)
00156 #define _mav_put_float(buf, wire_offset, b)    byte_copy_4(&buf[wire_offset], (const char *)&b)
00157 #define _mav_put_double(buf, wire_offset, b)   byte_copy_8(&buf[wire_offset], (const char *)&b)
00158 #else
00159 #define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
00160 #define _mav_put_int16_t(buf, wire_offset, b)  *(int16_t *)&buf[wire_offset] = b
00161 #define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
00162 #define _mav_put_int32_t(buf, wire_offset, b)  *(int32_t *)&buf[wire_offset] = b
00163 #define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
00164 #define _mav_put_int64_t(buf, wire_offset, b)  *(int64_t *)&buf[wire_offset] = b
00165 #define _mav_put_float(buf, wire_offset, b)    *(float *)&buf[wire_offset] = b
00166 #define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset] = b
00167 #endif
00168 
00169 /*
00170   like memcpy(), but if src is NULL, do a memset to zero
00171 */
00172 static inline void mav_array_memcpy(void *dest, const void *src, size_t n)
00173 {
00174         if (src == NULL) {
00175                 memset(dest, 0, n);
00176         } else {
00177                 memcpy(dest, src, n);
00178         }
00179 }
00180 
00181 /*
00182  * Place a char array into a buffer
00183  */
00184 static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
00185 {
00186         mav_array_memcpy(&buf[wire_offset], b, array_length);
00187 
00188 }
00189 
00190 /*
00191  * Place a uint8_t array into a buffer
00192  */
00193 static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
00194 {
00195         mav_array_memcpy(&buf[wire_offset], b, array_length);
00196 
00197 }
00198 
00199 /*
00200  * Place a int8_t array into a buffer
00201  */
00202 static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
00203 {
00204         mav_array_memcpy(&buf[wire_offset], b, array_length);
00205 
00206 }
00207 
00208 #if MAVLINK_NEED_BYTE_SWAP
00209 #define _MAV_PUT_ARRAY(TYPE, V) \
00210 static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
00211 { \
00212         if (b == NULL) { \
00213                 memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
00214         } else { \
00215                 uint16_t i; \
00216                 for (i=0; i<array_length; i++) { \
00217                         _mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
00218                 } \
00219         } \
00220 }
00221 #else
00222 #define _MAV_PUT_ARRAY(TYPE, V)                                 \
00223 static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
00224 { \
00225         mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
00226 }
00227 #endif
00228 
00229 _MAV_PUT_ARRAY(uint16_t, u16)
00230 _MAV_PUT_ARRAY(uint32_t, u32)
00231 _MAV_PUT_ARRAY(uint64_t, u64)
00232 _MAV_PUT_ARRAY(int16_t,  i16)
00233 _MAV_PUT_ARRAY(int32_t,  i32)
00234 _MAV_PUT_ARRAY(int64_t,  i64)
00235 _MAV_PUT_ARRAY(float,    f)
00236 _MAV_PUT_ARRAY(double,   d)
00237 
00238 #define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
00239 #define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
00240 #define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
00241 
00242 #if MAVLINK_NEED_BYTE_SWAP
00243 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
00244 static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
00245 { TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
00246 
00247 _MAV_MSG_RETURN_TYPE(uint16_t, 2)
00248 _MAV_MSG_RETURN_TYPE(int16_t,  2)
00249 _MAV_MSG_RETURN_TYPE(uint32_t, 4)
00250 _MAV_MSG_RETURN_TYPE(int32_t,  4)
00251 _MAV_MSG_RETURN_TYPE(uint64_t, 8)
00252 _MAV_MSG_RETURN_TYPE(int64_t,  8)
00253 _MAV_MSG_RETURN_TYPE(float,    4)
00254 _MAV_MSG_RETURN_TYPE(double,   8)
00255 
00256 #elif !MAVLINK_ALIGNED_FIELDS
00257 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
00258 static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
00259 { TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
00260 
00261 _MAV_MSG_RETURN_TYPE(uint16_t, 2)
00262 _MAV_MSG_RETURN_TYPE(int16_t,  2)
00263 _MAV_MSG_RETURN_TYPE(uint32_t, 4)
00264 _MAV_MSG_RETURN_TYPE(int32_t,  4)
00265 _MAV_MSG_RETURN_TYPE(uint64_t, 8)
00266 _MAV_MSG_RETURN_TYPE(int64_t,  8)
00267 _MAV_MSG_RETURN_TYPE(float,    4)
00268 _MAV_MSG_RETURN_TYPE(double,   8)
00269 #else // nicely aligned, no swap
00270 #define _MAV_MSG_RETURN_TYPE(TYPE) \
00271 static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
00272 { return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
00273 
00274 _MAV_MSG_RETURN_TYPE(uint16_t)
00275 _MAV_MSG_RETURN_TYPE(int16_t)
00276 _MAV_MSG_RETURN_TYPE(uint32_t)
00277 _MAV_MSG_RETURN_TYPE(int32_t)
00278 _MAV_MSG_RETURN_TYPE(uint64_t)
00279 _MAV_MSG_RETURN_TYPE(int64_t)
00280 _MAV_MSG_RETURN_TYPE(float)
00281 _MAV_MSG_RETURN_TYPE(double)
00282 #endif // MAVLINK_NEED_BYTE_SWAP
00283 
00284 static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, 
00285                                                      uint8_t array_length, uint8_t wire_offset)
00286 {
00287         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
00288         return array_length;
00289 }
00290 
00291 static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, 
00292                                                         uint8_t array_length, uint8_t wire_offset)
00293 {
00294         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
00295         return array_length;
00296 }
00297 
00298 static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, 
00299                                                        uint8_t array_length, uint8_t wire_offset)
00300 {
00301         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
00302         return array_length;
00303 }
00304 
00305 #if MAVLINK_NEED_BYTE_SWAP
00306 #define _MAV_RETURN_ARRAY(TYPE, V) \
00307 static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
00308                                                          uint8_t array_length, uint8_t wire_offset) \
00309 { \
00310         uint16_t i; \
00311         for (i=0; i<array_length; i++) { \
00312                 value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
00313         } \
00314         return array_length*sizeof(value[0]); \
00315 }
00316 #else
00317 #define _MAV_RETURN_ARRAY(TYPE, V)                                      \
00318 static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
00319                                                          uint8_t array_length, uint8_t wire_offset) \
00320 { \
00321         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
00322         return array_length*sizeof(TYPE); \
00323 }
00324 #endif
00325 
00326 _MAV_RETURN_ARRAY(uint16_t, u16)
00327 _MAV_RETURN_ARRAY(uint32_t, u32)
00328 _MAV_RETURN_ARRAY(uint64_t, u64)
00329 _MAV_RETURN_ARRAY(int16_t,  i16)
00330 _MAV_RETURN_ARRAY(int32_t,  i32)
00331 _MAV_RETURN_ARRAY(int64_t,  i64)
00332 _MAV_RETURN_ARRAY(float,    f)
00333 _MAV_RETURN_ARRAY(double,   d)
00334 
00335 #endif // _MAVLINK_PROTOCOL_H_