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