protocol.h

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