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