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