lib/main/MAVLink/protocol.h

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