flight/libraries/mavlink/v1.0/protocol.h

   1 #ifndef  _MAVLINK_PROTOCOL_H_
   2 #define  _MAVLINK_PROTOCOL_H_
   3
   4 #include "string.h"
   5 #include "mavlink_types.h"
   6
   7 /*
   8    If you want MAVLink on a system that is native big-endian,
   9    you need to define NATIVE_BIG_ENDIAN
  10  */
  11 #ifdef NATIVE_BIG_ENDIAN
  12 # define MAVLINK_NEED_BYTE_SWAP      (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
  13 #else
  14 # define MAVLINK_NEED_BYTE_SWAP      (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
  15 #endif
  16
  17 #ifndef MAVLINK_STACK_BUFFER
  18 #define MAVLINK_STACK_BUFFER         0
  19 #endif
  20
  21 #ifndef MAVLINK_AVOID_GCC_STACK_BUG
  22 # define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
  23 #endif
  24
  25 #ifndef MAVLINK_ASSERT
  26 #define MAVLINK_ASSERT(x)
  27 #endif
  28
  29 #ifndef MAVLINK_START_UART_SEND
  30 #define MAVLINK_START_UART_SEND(chan, length)
  31 #endif
  32
  33 #ifndef MAVLINK_END_UART_SEND
  34 #define MAVLINK_END_UART_SEND(chan, length)
  35 #endif
  36
  37 #ifdef MAVLINK_SEPARATE_HELPERS
  38 #define MAVLINK_HELPER
  39 #else
  40 #define MAVLINK_HELPER static inline
  41 #include "mavlink_helpers.h"
  42 #endif // MAVLINK_SEPARATE_HELPERS
  43
  44 /* always include the prototypes to ensure we don't get out of sync */
  45 MAVLINK_HELPER mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
  46 #if MAVLINK_CRC_EXTRA
  47 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id,
  48                                                       uint8_t chan, uint8_t length, uint8_t crc_extra);
  49 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id,
  50                                                  uint8_t length, uint8_t crc_extra);
  51 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  52 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
  53                                                     uint8_t length, uint8_t crc_extra);
  54 #endif
  55 #else
  56 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id,
  57                                                       uint8_t chan, uint8_t length);
  58 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id,
  59                                                  uint8_t length);
  60 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
  61 #endif // MAVLINK_CRC_EXTRA
  62 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
  63 MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t *msg);
  64 MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t *msg, uint8_t c);
  65 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t *r_message, mavlink_status_t *r_mavlink_status);
  66 MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index,
  67                                                uint8_t *r_bit_index, uint8_t *buffer);
  68 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  69 MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
  70 MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg);
  71 #endif
  72
  73 /**
  74  * @brief Get the required buffer size for this message
  75  */
  76 static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t *msg)
  77 {
  78     return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
  79 }
  80
  81 #if MAVLINK_NEED_BYTE_SWAP
  82 static inline void byte_swap_2(char *dst, const char *src)
  83 {
  84     dst[0] = src[1];
  85     dst[1] = src[0];
  86 }
  87 static inline void byte_swap_4(char *dst, const char *src)
  88 {
  89     dst[0] = src[3];
  90     dst[1] = src[2];
  91     dst[2] = src[1];
  92     dst[3] = src[0];
  93 }
  94 static inline void byte_swap_8(char *dst, const char *src)
  95 {
  96     dst[0] = src[7];
  97     dst[1] = src[6];
  98     dst[2] = src[5];
  99     dst[3] = src[4];
 100     dst[4] = src[3];
 101     dst[5] = src[2];
 102     dst[6] = src[1];
 103     dst[7] = src[0];
 104 }
 105 #elif !MAVLINK_ALIGNED_FIELDS
 106 static inline void byte_copy_2(char *dst, const char *src)
 107 {
 108     dst[0] = src[0];
 109     dst[1] = src[1];
 110 }
 111 static inline void byte_copy_4(char *dst, const char *src)
 112 {
 113     dst[0] = src[0];
 114     dst[1] = src[1];
 115     dst[2] = src[2];
 116     dst[3] = src[3];
 117 }
 118 static inline void byte_copy_8(char *dst, const char *src)
 119 {
 120     memcpy(dst, src, 8);
 121 }
 122 #endif // if MAVLINK_NEED_BYTE_SWAP
 123
 124 #define _mav_put_uint8_t(buf, wire_offset, b)  buf[wire_offset] = (uint8_t)b
 125 #define _mav_put_int8_t(buf, wire_offset, b)   buf[wire_offset] = (int8_t)b
 126 #define _mav_put_char(buf, wire_offset, b)     buf[wire_offset] = b
 127
 128 #if MAVLINK_NEED_BYTE_SWAP
 129 #define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
 130 #define _mav_put_int16_t(buf, wire_offset, b)  byte_swap_2(&buf[wire_offset], (const char *)&b)
 131 #define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
 132 #define _mav_put_int32_t(buf, wire_offset, b)  byte_swap_4(&buf[wire_offset], (const char *)&b)
 133 #define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
 134 #define _mav_put_int64_t(buf, wire_offset, b)  byte_swap_8(&buf[wire_offset], (const char *)&b)
 135 #define _mav_put_float(buf, wire_offset, b)    byte_swap_4(&buf[wire_offset], (const char *)&b)
 136 #define _mav_put_double(buf, wire_offset, b)   byte_swap_8(&buf[wire_offset], (const char *)&b)
 137 #elif !MAVLINK_ALIGNED_FIELDS
 138 #define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
 139 #define _mav_put_int16_t(buf, wire_offset, b)  byte_copy_2(&buf[wire_offset], (const char *)&b)
 140 #define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
 141 #define _mav_put_int32_t(buf, wire_offset, b)  byte_copy_4(&buf[wire_offset], (const char *)&b)
 142 #define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
 143 #define _mav_put_int64_t(buf, wire_offset, b)  byte_copy_8(&buf[wire_offset], (const char *)&b)
 144 #define _mav_put_float(buf, wire_offset, b)    byte_copy_4(&buf[wire_offset], (const char *)&b)
 145 #define _mav_put_double(buf, wire_offset, b)   byte_copy_8(&buf[wire_offset], (const char *)&b)
 146 #else
 147 #define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
 148 #define _mav_put_int16_t(buf, wire_offset, b)  *(int16_t *)&buf[wire_offset]  = b
 149 #define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
 150 #define _mav_put_int32_t(buf, wire_offset, b)  *(int32_t *)&buf[wire_offset]  = b
 151 #define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
 152 #define _mav_put_int64_t(buf, wire_offset, b)  *(int64_t *)&buf[wire_offset]  = b
 153 #define _mav_put_float(buf, wire_offset, b)    *(float *)&buf[wire_offset]    = b
 154 #define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset]   = b
 155 #endif // if MAVLINK_NEED_BYTE_SWAP
 156
 157 /*
 158    like memcpy(), but if src is NULL, do a memset to zero
 159  */
 160 static void mav_array_memcpy(void *dest, const void *src, size_t n)
 161 {
 162     if (src == NULL) {
 163         memset(dest, 0, n);
 164     } else {
 165         memcpy(dest, src, n);
 166     }
 167 }
 168
 169 /*
 170  * Place a char array into a buffer
 171  */
 172 static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
 173 {
 174     mav_array_memcpy(&buf[wire_offset], b, array_length);
 175 }
 176
 177 /*
 178  * Place a uint8_t array into a buffer
 179  */
 180 static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
 181 {
 182     mav_array_memcpy(&buf[wire_offset], b, array_length);
 183 }
 184
 185 /*
 186  * Place a int8_t array into a buffer
 187  */
 188 static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
 189 {
 190     mav_array_memcpy(&buf[wire_offset], b, array_length);
 191 }
 192
 193 #if MAVLINK_NEED_BYTE_SWAP
 194 #define _MAV_PUT_ARRAY(TYPE, V) \
 195     static inline void _mav_put_##TYPE##_array(char *buf, uint8_t wire_offset, const TYPE * b, uint8_t array_length) \
 196     { \
 197         if (b == NULL) { \
 198             memset(&buf[wire_offset], 0, array_length * sizeof(TYPE)); \
 199         } \
 200         else { \
 201             uint16_t i; \
 202             for (i = 0; i < array_length; i++) { \
 203                 _mav_put_##TYPE(buf, wire_offset + (i * sizeof(TYPE)), b[i]); \
 204             } \
 205         } \
 206     }
 207 #else
 208 #define _MAV_PUT_ARRAY(TYPE, V)                                 \
 209     static inline void _mav_put_##TYPE##_array(char *buf, uint8_t wire_offset, const TYPE * b, uint8_t array_length) \
 210     { \
 211         mav_array_memcpy(&buf[wire_offset], b, array_length * sizeof(TYPE)); \
 212     }
 213 #endif
 214
 215 _MAV_PUT_ARRAY(uint16_t, u16)
 216 _MAV_PUT_ARRAY(uint32_t, u32)
 217 _MAV_PUT_ARRAY(uint64_t, u64)
 218 _MAV_PUT_ARRAY(int16_t, i16)
 219 _MAV_PUT_ARRAY(int32_t, i32)
 220 _MAV_PUT_ARRAY(int64_t, i64)
 221 _MAV_PUT_ARRAY(float, f)
 222 _MAV_PUT_ARRAY(double, d)
 223
 224 #define _MAV_RETURN_char(msg, wire_offset)    (const char)_MAV_PAYLOAD(msg)[wire_offset]
 225 #define _MAV_RETURN_int8_t(msg, wire_offset)  (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
 226 #define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
 227
 228 #if MAVLINK_NEED_BYTE_SWAP
 229 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
 230     static inline TYPE _MAV_RETURN_##TYPE(const mavlink_message_t * msg, uint8_t ofs) \
 231     { TYPE r; byte_swap_##SIZE((char *)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
 232
 233 _MAV_MSG_RETURN_TYPE(uint16_t, 2)
 234 _MAV_MSG_RETURN_TYPE(int16_t, 2)
 235 _MAV_MSG_RETURN_TYPE(uint32_t, 4)
 236 _MAV_MSG_RETURN_TYPE(int32_t, 4)
 237 _MAV_MSG_RETURN_TYPE(uint64_t, 8)
 238 _MAV_MSG_RETURN_TYPE(int64_t, 8)
 239 _MAV_MSG_RETURN_TYPE(float, 4)
 240 _MAV_MSG_RETURN_TYPE(double, 8)
 241
 242 #elif !MAVLINK_ALIGNED_FIELDS
 243 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
 244     static inline TYPE _MAV_RETURN_##TYPE(const mavlink_message_t * msg, uint8_t ofs) \
 245     { TYPE r; byte_copy_##SIZE((char *)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
 246
 247 _MAV_MSG_RETURN_TYPE(uint16_t, 2)
 248 _MAV_MSG_RETURN_TYPE(int16_t, 2)
 249 _MAV_MSG_RETURN_TYPE(uint32_t, 4)
 250 _MAV_MSG_RETURN_TYPE(int32_t, 4)
 251 _MAV_MSG_RETURN_TYPE(uint64_t, 8)
 252 _MAV_MSG_RETURN_TYPE(int64_t, 8)
 253 _MAV_MSG_RETURN_TYPE(float, 4)
 254 _MAV_MSG_RETURN_TYPE(double, 8)
 255 #else // nicely aligned, no swap
 256 #define _MAV_MSG_RETURN_TYPE(TYPE) \
 257     static inline TYPE _MAV_RETURN_##TYPE(const mavlink_message_t * msg, uint8_t ofs) \
 258     { return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]); }
 259
 260 _MAV_MSG_RETURN_TYPE(uint16_t)
 261 _MAV_MSG_RETURN_TYPE(int16_t)
 262 _MAV_MSG_RETURN_TYPE(uint32_t)
 263 _MAV_MSG_RETURN_TYPE(int32_t)
 264 _MAV_MSG_RETURN_TYPE(uint64_t)
 265 _MAV_MSG_RETURN_TYPE(int64_t)
 266 _MAV_MSG_RETURN_TYPE(float)
 267 _MAV_MSG_RETURN_TYPE(double)
 268 #endif // MAVLINK_NEED_BYTE_SWAP
 269
 270 static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value,
 271                                               uint8_t array_length, uint8_t wire_offset)
 272 {
 273     memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
 274     return array_length;
 275 }
 276
 277 static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value,
 278                                                  uint8_t array_length, uint8_t wire_offset)
 279 {
 280     memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
 281     return array_length;
 282 }
 283
 284 static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value,
 285                                                 uint8_t array_length, uint8_t wire_offset)
 286 {
 287     memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
 288     return array_length;
 289 }
 290
 291 #if MAVLINK_NEED_BYTE_SWAP
 292 #define _MAV_RETURN_ARRAY(TYPE, V) \
 293     static inline uint16_t _MAV_RETURN_##TYPE##_array(const mavlink_message_t * msg, TYPE * value, \
 294                                                       uint8_t array_length, uint8_t wire_offset) \
 295     { \
 296         uint16_t i; \
 297         for (i = 0; i < array_length; i++) { \
 298             value[i] = _MAV_RETURN_##TYPE(msg, wire_offset + (i * sizeof(value[0]))); \
 299         } \
 300         return array_length * sizeof(value[0]); \
 301     }
 302 #else
 303 #define _MAV_RETURN_ARRAY(TYPE, V)                                      \
 304     static inline uint16_t _MAV_RETURN_##TYPE##_array(const mavlink_message_t * msg, TYPE * value, \
 305                                                       uint8_t array_length, uint8_t wire_offset) \
 306     { \
 307         memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length * sizeof(TYPE)); \
 308         return array_length * sizeof(TYPE); \
 309     }
 310 #endif
 311
 312 _MAV_RETURN_ARRAY(uint16_t, u16)
 313 _MAV_RETURN_ARRAY(uint32_t, u32)
 314 _MAV_RETURN_ARRAY(uint64_t, u64)
 315 _MAV_RETURN_ARRAY(int16_t, i16)
 316 _MAV_RETURN_ARRAY(int32_t, i32)
 317 _MAV_RETURN_ARRAY(int64_t, i64)
 318 _MAV_RETURN_ARRAY(float, f)
 319 _MAV_RETURN_ARRAY(double, d)
 320
 321 #endif // _MAVLINK_PROTOCOL_H_