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