source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/include/protocol.h @ 82

Last change on this file since 82 was 75, checked in by Thomas Fuhrmann, 5 years ago

Change the version of mavlink generated messages and rename it to include

File size: 12.4 KB
Line 
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 */
92static 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
98static inline void byte_swap_2(char *dst, const char *src)
99{
100        dst[0] = src[1];
101        dst[1] = src[0];
102}
103static 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}
110static 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
122static inline void byte_copy_2(char *dst, const char *src)
123{
124        dst[0] = src[0];
125        dst[1] = src[1];
126}
127static 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}
134static 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*/
176static 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 */
188static 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 */
197static 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 */
206static 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) \
214static 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)                                 \
227static 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) \
248static 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) \
262static 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) \
275static 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
288static 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
295static 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
302static 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) \
311static 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)                                      \
322static 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_
Note: See TracBrowser for help on using the repository browser.