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