Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

MAVlink/include/protocol.h@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Working version with priorities set and update time display

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #ifndef _MAVLINK_PROTOCOL_H_
shimniok 0:a6a169de725f 2 #define _MAVLINK_PROTOCOL_H_
shimniok 0:a6a169de725f 3
shimniok 0:a6a169de725f 4 #include "string.h"
shimniok 0:a6a169de725f 5 #include "checksum.h"
shimniok 0:a6a169de725f 6
shimniok 0:a6a169de725f 7 #include "mavlink_types.h"
shimniok 0:a6a169de725f 8
shimniok 0:a6a169de725f 9
shimniok 0:a6a169de725f 10 /**
shimniok 0:a6a169de725f 11 * @brief Initialize the communication stack
shimniok 0:a6a169de725f 12 *
shimniok 0:a6a169de725f 13 * This function has to be called before using commParseBuffer() to initialize the different status registers.
shimniok 0:a6a169de725f 14 *
shimniok 0:a6a169de725f 15 * @return Will initialize the different buffers and status registers.
shimniok 0:a6a169de725f 16 */
shimniok 0:a6a169de725f 17 static void mavlink_parse_state_initialize(mavlink_status_t* initStatus)
shimniok 0:a6a169de725f 18 {
shimniok 0:a6a169de725f 19 if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1))
shimniok 0:a6a169de725f 20 {
shimniok 0:a6a169de725f 21 initStatus->ck_a = 0;
shimniok 0:a6a169de725f 22 initStatus->ck_b = 0;
shimniok 0:a6a169de725f 23 initStatus->msg_received = 0;
shimniok 0:a6a169de725f 24 initStatus->buffer_overrun = 0;
shimniok 0:a6a169de725f 25 initStatus->parse_error = 0;
shimniok 0:a6a169de725f 26 initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT;
shimniok 0:a6a169de725f 27 initStatus->packet_idx = 0;
shimniok 0:a6a169de725f 28 initStatus->packet_rx_drop_count = 0;
shimniok 0:a6a169de725f 29 initStatus->packet_rx_success_count = 0;
shimniok 0:a6a169de725f 30 initStatus->current_rx_seq = 0;
shimniok 0:a6a169de725f 31 initStatus->current_tx_seq = 0;
shimniok 0:a6a169de725f 32 }
shimniok 0:a6a169de725f 33 }
shimniok 0:a6a169de725f 34
shimniok 0:a6a169de725f 35 static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
shimniok 0:a6a169de725f 36 {
shimniok 0:a6a169de725f 37 static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 38 return &m_mavlink_status[chan];
shimniok 0:a6a169de725f 39 }
shimniok 0:a6a169de725f 40
shimniok 0:a6a169de725f 41 /**
shimniok 0:a6a169de725f 42 * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
shimniok 0:a6a169de725f 43 *
shimniok 0:a6a169de725f 44 * This function calculates the checksum and sets length and aircraft id correctly.
shimniok 0:a6a169de725f 45 * It assumes that the message id and the payload are already correctly set.
shimniok 0:a6a169de725f 46 *
shimniok 0:a6a169de725f 47 * @warning This function implicitely assumes the message is sent over channel zero.
shimniok 0:a6a169de725f 48 * if the message is sent over a different channel it will reach the receiver
shimniok 0:a6a169de725f 49 * without error, BUT the sequence number might be wrong due to the wrong
shimniok 0:a6a169de725f 50 * channel sequence counter. This will result is wrongly reported excessive
shimniok 0:a6a169de725f 51 * packet loss. Please use @see mavlink_{pack|encode}_headerless and then
shimniok 0:a6a169de725f 52 * @see mavlink_finalize_message_chan before sending for a correct channel
shimniok 0:a6a169de725f 53 * assignment. Please note that the mavlink_msg_xxx_pack and encode functions
shimniok 0:a6a169de725f 54 * assign channel zero as default and thus induce possible loss counter errors.\
shimniok 0:a6a169de725f 55 * They have been left to ensure code compatibility.
shimniok 0:a6a169de725f 56 *
shimniok 0:a6a169de725f 57 * @see mavlink_finalize_message_chan
shimniok 0:a6a169de725f 58 * @param msg Message to finalize
shimniok 0:a6a169de725f 59 * @param system_id Id of the sending (this) system, 1-127
shimniok 0:a6a169de725f 60 * @param length Message length, usually just the counter incremented while packing the message
shimniok 0:a6a169de725f 61 */
shimniok 0:a6a169de725f 62 static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length)
shimniok 0:a6a169de725f 63 {
shimniok 0:a6a169de725f 64 // This code part is the same for all messages;
shimniok 0:a6a169de725f 65 uint16_t checksum;
shimniok 0:a6a169de725f 66 msg->len = length;
shimniok 0:a6a169de725f 67 msg->sysid = system_id;
shimniok 0:a6a169de725f 68 msg->compid = component_id;
shimniok 0:a6a169de725f 69 // One sequence number per component
shimniok 0:a6a169de725f 70 msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq;
shimniok 0:a6a169de725f 71 mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
shimniok 0:a6a169de725f 72 checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
shimniok 0:a6a169de725f 73 msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
shimniok 0:a6a169de725f 74 msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
shimniok 0:a6a169de725f 75
shimniok 0:a6a169de725f 76 return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES;
shimniok 0:a6a169de725f 77 }
shimniok 0:a6a169de725f 78
shimniok 0:a6a169de725f 79 /**
shimniok 0:a6a169de725f 80 * @brief Finalize a MAVLink message with channel assignment
shimniok 0:a6a169de725f 81 *
shimniok 0:a6a169de725f 82 * This function calculates the checksum and sets length and aircraft id correctly.
shimniok 0:a6a169de725f 83 * It assumes that the message id and the payload are already correctly set. This function
shimniok 0:a6a169de725f 84 * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
shimniok 0:a6a169de725f 85 * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
shimniok 0:a6a169de725f 86 *
shimniok 0:a6a169de725f 87 * @param msg Message to finalize
shimniok 0:a6a169de725f 88 * @param system_id Id of the sending (this) system, 1-127
shimniok 0:a6a169de725f 89 * @param length Message length, usually just the counter incremented while packing the message
shimniok 0:a6a169de725f 90 */
shimniok 0:a6a169de725f 91 static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length)
shimniok 0:a6a169de725f 92 {
shimniok 0:a6a169de725f 93 // This code part is the same for all messages;
shimniok 0:a6a169de725f 94 uint16_t checksum;
shimniok 0:a6a169de725f 95 msg->len = length;
shimniok 0:a6a169de725f 96 msg->sysid = system_id;
shimniok 0:a6a169de725f 97 msg->compid = component_id;
shimniok 0:a6a169de725f 98 // One sequence number per component
shimniok 0:a6a169de725f 99 msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
shimniok 0:a6a169de725f 100 mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
shimniok 0:a6a169de725f 101 checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
shimniok 0:a6a169de725f 102 msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
shimniok 0:a6a169de725f 103 msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
shimniok 0:a6a169de725f 104
shimniok 0:a6a169de725f 105 return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES;
shimniok 0:a6a169de725f 106 }
shimniok 0:a6a169de725f 107
shimniok 0:a6a169de725f 108 /**
shimniok 0:a6a169de725f 109 * @brief Pack a message to send it over a serial byte stream
shimniok 0:a6a169de725f 110 */
shimniok 0:a6a169de725f 111 static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg)
shimniok 0:a6a169de725f 112 {
shimniok 0:a6a169de725f 113 *(buffer+0) = MAVLINK_STX; ///< Start transmit
shimniok 0:a6a169de725f 114 memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload
shimniok 0:a6a169de725f 115 *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a;
shimniok 0:a6a169de725f 116 *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b;
shimniok 0:a6a169de725f 117 return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
shimniok 0:a6a169de725f 118 //return 0;
shimniok 0:a6a169de725f 119 }
shimniok 0:a6a169de725f 120
shimniok 0:a6a169de725f 121 /**
shimniok 0:a6a169de725f 122 * @brief Get the required buffer size for this message
shimniok 0:a6a169de725f 123 */
shimniok 0:a6a169de725f 124 static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
shimniok 0:a6a169de725f 125 {
shimniok 0:a6a169de725f 126 return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
shimniok 0:a6a169de725f 127 }
shimniok 0:a6a169de725f 128
shimniok 0:a6a169de725f 129 union checksum_ {
shimniok 0:a6a169de725f 130 uint16_t s;
shimniok 0:a6a169de725f 131 uint8_t c[2];
shimniok 0:a6a169de725f 132 };
shimniok 0:a6a169de725f 133
shimniok 0:a6a169de725f 134 union __mavlink_bitfield {
shimniok 0:a6a169de725f 135 uint8_t uint8;
shimniok 0:a6a169de725f 136 int8_t int8;
shimniok 0:a6a169de725f 137 uint16_t uint16;
shimniok 0:a6a169de725f 138 int16_t int16;
shimniok 0:a6a169de725f 139 uint32_t uint32;
shimniok 0:a6a169de725f 140 int32_t int32;
shimniok 0:a6a169de725f 141 };
shimniok 0:a6a169de725f 142
shimniok 0:a6a169de725f 143
shimniok 0:a6a169de725f 144 static inline void mavlink_start_checksum(mavlink_message_t* msg)
shimniok 0:a6a169de725f 145 {
shimniok 0:a6a169de725f 146 union checksum_ ck;
shimniok 0:a6a169de725f 147 crc_init(&(ck.s));
shimniok 0:a6a169de725f 148 msg->ck_a = ck.c[0];
shimniok 0:a6a169de725f 149 msg->ck_b = ck.c[1];
shimniok 0:a6a169de725f 150 }
shimniok 0:a6a169de725f 151
shimniok 0:a6a169de725f 152 static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
shimniok 0:a6a169de725f 153 {
shimniok 0:a6a169de725f 154 union checksum_ ck;
shimniok 0:a6a169de725f 155 ck.c[0] = msg->ck_a;
shimniok 0:a6a169de725f 156 ck.c[1] = msg->ck_b;
shimniok 0:a6a169de725f 157 crc_accumulate(c, &(ck.s));
shimniok 0:a6a169de725f 158 msg->ck_a = ck.c[0];
shimniok 0:a6a169de725f 159 msg->ck_b = ck.c[1];
shimniok 0:a6a169de725f 160 }
shimniok 0:a6a169de725f 161
shimniok 0:a6a169de725f 162 /**
shimniok 0:a6a169de725f 163 * This is a convenience function which handles the complete MAVLink parsing.
shimniok 0:a6a169de725f 164 * the function will parse one byte at a time and return the complete packet once
shimniok 0:a6a169de725f 165 * it could be successfully decoded. Checksum and other failures will be silently
shimniok 0:a6a169de725f 166 * ignored.
shimniok 0:a6a169de725f 167 *
shimniok 0:a6a169de725f 168 * @param chan ID of the current channel. This allows to parse different channels with this function.
shimniok 0:a6a169de725f 169 * a channel is not a physical message channel like a serial port, but a logic partition of
shimniok 0:a6a169de725f 170 * the communication streams in this case. COMM_NB is the limit for the number of channels
shimniok 0:a6a169de725f 171 * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
shimniok 0:a6a169de725f 172 * @param c The char to barse
shimniok 0:a6a169de725f 173 *
shimniok 0:a6a169de725f 174 * @param returnMsg NULL if no message could be decoded, the message data else
shimniok 0:a6a169de725f 175 * @return 0 if no message could be decoded, 1 else
shimniok 0:a6a169de725f 176 *
shimniok 0:a6a169de725f 177 * A typical use scenario of this function call is:
shimniok 0:a6a169de725f 178 *
shimniok 0:a6a169de725f 179 * @code
shimniok 0:a6a169de725f 180 * #include <inttypes.h> // For fixed-width uint8_t type
shimniok 0:a6a169de725f 181 *
shimniok 0:a6a169de725f 182 * mavlink_message_t msg;
shimniok 0:a6a169de725f 183 * int chan = 0;
shimniok 0:a6a169de725f 184 *
shimniok 0:a6a169de725f 185 *
shimniok 0:a6a169de725f 186 * while(serial.bytesAvailable > 0)
shimniok 0:a6a169de725f 187 * {
shimniok 0:a6a169de725f 188 * uint8_t byte = serial.getNextByte();
shimniok 0:a6a169de725f 189 * if (mavlink_parse_char(chan, byte, &msg))
shimniok 0:a6a169de725f 190 * {
shimniok 0:a6a169de725f 191 * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
shimniok 0:a6a169de725f 192 * }
shimniok 0:a6a169de725f 193 * }
shimniok 0:a6a169de725f 194 *
shimniok 0:a6a169de725f 195 *
shimniok 0:a6a169de725f 196 * @endcode
shimniok 0:a6a169de725f 197 */
shimniok 0:a6a169de725f 198 static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
shimniok 0:a6a169de725f 199 {
shimniok 0:a6a169de725f 200 static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 201
shimniok 0:a6a169de725f 202 // Initializes only once, values keep unchanged after first initialization
shimniok 0:a6a169de725f 203 mavlink_parse_state_initialize(mavlink_get_channel_status(chan));
shimniok 0:a6a169de725f 204
shimniok 0:a6a169de725f 205 mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
shimniok 0:a6a169de725f 206 mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
shimniok 0:a6a169de725f 207 int bufferIndex = 0;
shimniok 0:a6a169de725f 208
shimniok 0:a6a169de725f 209 status->msg_received = 0;
shimniok 0:a6a169de725f 210
shimniok 0:a6a169de725f 211 switch (status->parse_state)
shimniok 0:a6a169de725f 212 {
shimniok 0:a6a169de725f 213 case MAVLINK_PARSE_STATE_UNINIT:
shimniok 0:a6a169de725f 214 case MAVLINK_PARSE_STATE_IDLE:
shimniok 0:a6a169de725f 215 if (c == MAVLINK_STX)
shimniok 0:a6a169de725f 216 {
shimniok 0:a6a169de725f 217 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
shimniok 0:a6a169de725f 218 mavlink_start_checksum(rxmsg);
shimniok 0:a6a169de725f 219 }
shimniok 0:a6a169de725f 220 break;
shimniok 0:a6a169de725f 221
shimniok 0:a6a169de725f 222 case MAVLINK_PARSE_STATE_GOT_STX:
shimniok 0:a6a169de725f 223 if (status->msg_received)
shimniok 0:a6a169de725f 224 {
shimniok 0:a6a169de725f 225 status->buffer_overrun++;
shimniok 0:a6a169de725f 226 status->parse_error++;
shimniok 0:a6a169de725f 227 status->msg_received = 0;
shimniok 0:a6a169de725f 228 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 229 }
shimniok 0:a6a169de725f 230 else
shimniok 0:a6a169de725f 231 {
shimniok 0:a6a169de725f 232 // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
shimniok 0:a6a169de725f 233 rxmsg->len = c;
shimniok 0:a6a169de725f 234 status->packet_idx = 0;
shimniok 0:a6a169de725f 235 mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 236 status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
shimniok 0:a6a169de725f 237 }
shimniok 0:a6a169de725f 238 break;
shimniok 0:a6a169de725f 239
shimniok 0:a6a169de725f 240 case MAVLINK_PARSE_STATE_GOT_LENGTH:
shimniok 0:a6a169de725f 241 rxmsg->seq = c;
shimniok 0:a6a169de725f 242 mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 243 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
shimniok 0:a6a169de725f 244 break;
shimniok 0:a6a169de725f 245
shimniok 0:a6a169de725f 246 case MAVLINK_PARSE_STATE_GOT_SEQ:
shimniok 0:a6a169de725f 247 rxmsg->sysid = c;
shimniok 0:a6a169de725f 248 mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 249 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
shimniok 0:a6a169de725f 250 break;
shimniok 0:a6a169de725f 251
shimniok 0:a6a169de725f 252 case MAVLINK_PARSE_STATE_GOT_SYSID:
shimniok 0:a6a169de725f 253 rxmsg->compid = c;
shimniok 0:a6a169de725f 254 mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 255 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
shimniok 0:a6a169de725f 256 break;
shimniok 0:a6a169de725f 257
shimniok 0:a6a169de725f 258 case MAVLINK_PARSE_STATE_GOT_COMPID:
shimniok 0:a6a169de725f 259 rxmsg->msgid = c;
shimniok 0:a6a169de725f 260 mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 261 if (rxmsg->len == 0)
shimniok 0:a6a169de725f 262 {
shimniok 0:a6a169de725f 263 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
shimniok 0:a6a169de725f 264 }
shimniok 0:a6a169de725f 265 else
shimniok 0:a6a169de725f 266 {
shimniok 0:a6a169de725f 267 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
shimniok 0:a6a169de725f 268 }
shimniok 0:a6a169de725f 269 break;
shimniok 0:a6a169de725f 270
shimniok 0:a6a169de725f 271 case MAVLINK_PARSE_STATE_GOT_MSGID:
shimniok 0:a6a169de725f 272 rxmsg->payload[status->packet_idx++] = c;
shimniok 0:a6a169de725f 273 mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 274 if (status->packet_idx == rxmsg->len)
shimniok 0:a6a169de725f 275 {
shimniok 0:a6a169de725f 276 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
shimniok 0:a6a169de725f 277 }
shimniok 0:a6a169de725f 278 break;
shimniok 0:a6a169de725f 279
shimniok 0:a6a169de725f 280 case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
shimniok 0:a6a169de725f 281 if (c != rxmsg->ck_a)
shimniok 0:a6a169de725f 282 {
shimniok 0:a6a169de725f 283 // Check first checksum byte
shimniok 0:a6a169de725f 284 status->parse_error++;
shimniok 0:a6a169de725f 285 status->msg_received = 0;
shimniok 0:a6a169de725f 286 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 287 if (c == MAVLINK_STX)
shimniok 0:a6a169de725f 288 {
shimniok 0:a6a169de725f 289 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
shimniok 0:a6a169de725f 290 mavlink_start_checksum(rxmsg);
shimniok 0:a6a169de725f 291 }
shimniok 0:a6a169de725f 292 }
shimniok 0:a6a169de725f 293 else
shimniok 0:a6a169de725f 294 {
shimniok 0:a6a169de725f 295 status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
shimniok 0:a6a169de725f 296 }
shimniok 0:a6a169de725f 297 break;
shimniok 0:a6a169de725f 298
shimniok 0:a6a169de725f 299 case MAVLINK_PARSE_STATE_GOT_CRC1:
shimniok 0:a6a169de725f 300 if (c != rxmsg->ck_b)
shimniok 0:a6a169de725f 301 {// Check second checksum byte
shimniok 0:a6a169de725f 302 status->parse_error++;
shimniok 0:a6a169de725f 303 status->msg_received = 0;
shimniok 0:a6a169de725f 304 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 305 if (c == MAVLINK_STX)
shimniok 0:a6a169de725f 306 {
shimniok 0:a6a169de725f 307 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
shimniok 0:a6a169de725f 308 mavlink_start_checksum(rxmsg);
shimniok 0:a6a169de725f 309 }
shimniok 0:a6a169de725f 310 }
shimniok 0:a6a169de725f 311 else
shimniok 0:a6a169de725f 312 {
shimniok 0:a6a169de725f 313 // Successfully got message
shimniok 0:a6a169de725f 314 status->msg_received = 1;
shimniok 0:a6a169de725f 315 status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 316 memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
shimniok 0:a6a169de725f 317 }
shimniok 0:a6a169de725f 318 break;
shimniok 0:a6a169de725f 319 }
shimniok 0:a6a169de725f 320
shimniok 0:a6a169de725f 321 bufferIndex++;
shimniok 0:a6a169de725f 322 // If a message has been sucessfully decoded, check index
shimniok 0:a6a169de725f 323 if (status->msg_received == 1)
shimniok 0:a6a169de725f 324 {
shimniok 0:a6a169de725f 325 //while(status->current_seq != rxmsg->seq)
shimniok 0:a6a169de725f 326 //{
shimniok 0:a6a169de725f 327 // status->packet_rx_drop_count++;
shimniok 0:a6a169de725f 328 // status->current_seq++;
shimniok 0:a6a169de725f 329 //}
shimniok 0:a6a169de725f 330 status->current_rx_seq = rxmsg->seq;
shimniok 0:a6a169de725f 331 // Initial condition: If no packet has been received so far, drop count is undefined
shimniok 0:a6a169de725f 332 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
shimniok 0:a6a169de725f 333 // Count this packet as received
shimniok 0:a6a169de725f 334 status->packet_rx_success_count++;
shimniok 0:a6a169de725f 335 }
shimniok 0:a6a169de725f 336
shimniok 0:a6a169de725f 337 r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
shimniok 0:a6a169de725f 338 r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
shimniok 0:a6a169de725f 339 r_mavlink_status->packet_rx_drop_count = status->parse_error;
shimniok 0:a6a169de725f 340 status->parse_error = 0;
shimniok 0:a6a169de725f 341 return status->msg_received;
shimniok 0:a6a169de725f 342 }
shimniok 0:a6a169de725f 343
shimniok 0:a6a169de725f 344
shimniok 0:a6a169de725f 345 /**
shimniok 0:a6a169de725f 346 * This is a convenience function which handles the complete MAVLink parsing.
shimniok 0:a6a169de725f 347 * the function will parse one byte at a time and return the complete packet once
shimniok 0:a6a169de725f 348 * it could be successfully decoded. Checksum and other failures will be silently
shimniok 0:a6a169de725f 349 * ignored.
shimniok 0:a6a169de725f 350 *
shimniok 0:a6a169de725f 351 * @param chan ID of the current channel. This allows to parse different channels with this function.
shimniok 0:a6a169de725f 352 * a channel is not a physical message channel like a serial port, but a logic partition of
shimniok 0:a6a169de725f 353 * the communication streams in this case. COMM_NB is the limit for the number of channels
shimniok 0:a6a169de725f 354 * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
shimniok 0:a6a169de725f 355 * @param c The char to barse
shimniok 0:a6a169de725f 356 *
shimniok 0:a6a169de725f 357 * @param returnMsg NULL if no message could be decoded, the message data else
shimniok 0:a6a169de725f 358 * @return 0 if no message could be decoded, 1 else
shimniok 0:a6a169de725f 359 *
shimniok 0:a6a169de725f 360 * A typical use scenario of this function call is:
shimniok 0:a6a169de725f 361 *
shimniok 0:a6a169de725f 362 * @code
shimniok 0:a6a169de725f 363 * #include <inttypes.h> // For fixed-width uint8_t type
shimniok 0:a6a169de725f 364 *
shimniok 0:a6a169de725f 365 * mavlink_message_t msg;
shimniok 0:a6a169de725f 366 * int chan = 0;
shimniok 0:a6a169de725f 367 *
shimniok 0:a6a169de725f 368 *
shimniok 0:a6a169de725f 369 * while(serial.bytesAvailable > 0)
shimniok 0:a6a169de725f 370 * {
shimniok 0:a6a169de725f 371 * uint8_t byte = serial.getNextByte();
shimniok 0:a6a169de725f 372 * if (mavlink_parse_char(chan, byte, &msg))
shimniok 0:a6a169de725f 373 * {
shimniok 0:a6a169de725f 374 * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
shimniok 0:a6a169de725f 375 * }
shimniok 0:a6a169de725f 376 * }
shimniok 0:a6a169de725f 377 *
shimniok 0:a6a169de725f 378 *
shimniok 0:a6a169de725f 379 * @endcode
shimniok 0:a6a169de725f 380 */
shimniok 0:a6a169de725f 381
shimniok 0:a6a169de725f 382 #define MAVLINK_PACKET_START_CANDIDATES 50
shimniok 0:a6a169de725f 383 /*
shimniok 0:a6a169de725f 384 static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
shimniok 0:a6a169de725f 385 {
shimniok 0:a6a169de725f 386 static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 387 static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2];
shimniok 0:a6a169de725f 388 static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 389 static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 390 static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES];
shimniok 0:a6a169de725f 391 static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 392 static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 393
shimniok 0:a6a169de725f 394 // Set a packet start candidate index if sign is start sign
shimniok 0:a6a169de725f 395 if (c == MAVLINK_STX)
shimniok 0:a6a169de725f 396 {
shimniok 0:a6a169de725f 397 m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan];
shimniok 0:a6a169de725f 398 }
shimniok 0:a6a169de725f 399
shimniok 0:a6a169de725f 400 // Parse normally, if a CRC mismatch occurs retry with the next packet index
shimniok 0:a6a169de725f 401 }
shimniok 0:a6a169de725f 402 // static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 403 // static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
shimniok 0:a6a169de725f 404 //// Initializes only once, values keep unchanged after first initialization
shimniok 0:a6a169de725f 405 // mavlink_parse_state_initialize(&m_mavlink_status[chan]);
shimniok 0:a6a169de725f 406 //
shimniok 0:a6a169de725f 407 //mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
shimniok 0:a6a169de725f 408 //mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status
shimniok 0:a6a169de725f 409 //int bufferIndex = 0;
shimniok 0:a6a169de725f 410 //
shimniok 0:a6a169de725f 411 //status->msg_received = 0;
shimniok 0:a6a169de725f 412 //
shimniok 0:a6a169de725f 413 //switch (status->parse_state)
shimniok 0:a6a169de725f 414 //{
shimniok 0:a6a169de725f 415 //case MAVLINK_PARSE_STATE_UNINIT:
shimniok 0:a6a169de725f 416 //case MAVLINK_PARSE_STATE_IDLE:
shimniok 0:a6a169de725f 417 // if (c == MAVLINK_STX)
shimniok 0:a6a169de725f 418 // {
shimniok 0:a6a169de725f 419 // status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
shimniok 0:a6a169de725f 420 // mavlink_start_checksum(rxmsg);
shimniok 0:a6a169de725f 421 // }
shimniok 0:a6a169de725f 422 // break;
shimniok 0:a6a169de725f 423 //
shimniok 0:a6a169de725f 424 //case MAVLINK_PARSE_STATE_GOT_STX:
shimniok 0:a6a169de725f 425 // if (status->msg_received)
shimniok 0:a6a169de725f 426 // {
shimniok 0:a6a169de725f 427 // status->buffer_overrun++;
shimniok 0:a6a169de725f 428 // status->parse_error++;
shimniok 0:a6a169de725f 429 // status->msg_received = 0;
shimniok 0:a6a169de725f 430 // status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 431 // }
shimniok 0:a6a169de725f 432 // else
shimniok 0:a6a169de725f 433 // {
shimniok 0:a6a169de725f 434 // // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
shimniok 0:a6a169de725f 435 // rxmsg->len = c;
shimniok 0:a6a169de725f 436 // status->packet_idx = 0;
shimniok 0:a6a169de725f 437 // mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 438 // status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
shimniok 0:a6a169de725f 439 // }
shimniok 0:a6a169de725f 440 // break;
shimniok 0:a6a169de725f 441 //
shimniok 0:a6a169de725f 442 //case MAVLINK_PARSE_STATE_GOT_LENGTH:
shimniok 0:a6a169de725f 443 // rxmsg->seq = c;
shimniok 0:a6a169de725f 444 // mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 445 // status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
shimniok 0:a6a169de725f 446 // break;
shimniok 0:a6a169de725f 447 //
shimniok 0:a6a169de725f 448 //case MAVLINK_PARSE_STATE_GOT_SEQ:
shimniok 0:a6a169de725f 449 // rxmsg->sysid = c;
shimniok 0:a6a169de725f 450 // mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 451 // status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
shimniok 0:a6a169de725f 452 // break;
shimniok 0:a6a169de725f 453 //
shimniok 0:a6a169de725f 454 //case MAVLINK_PARSE_STATE_GOT_SYSID:
shimniok 0:a6a169de725f 455 // rxmsg->compid = c;
shimniok 0:a6a169de725f 456 // mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 457 // status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
shimniok 0:a6a169de725f 458 // break;
shimniok 0:a6a169de725f 459 //
shimniok 0:a6a169de725f 460 //case MAVLINK_PARSE_STATE_GOT_COMPID:
shimniok 0:a6a169de725f 461 // rxmsg->msgid = c;
shimniok 0:a6a169de725f 462 // mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 463 // if (rxmsg->len == 0)
shimniok 0:a6a169de725f 464 // {
shimniok 0:a6a169de725f 465 // status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
shimniok 0:a6a169de725f 466 // }
shimniok 0:a6a169de725f 467 // else
shimniok 0:a6a169de725f 468 // {
shimniok 0:a6a169de725f 469 // status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
shimniok 0:a6a169de725f 470 // }
shimniok 0:a6a169de725f 471 // break;
shimniok 0:a6a169de725f 472 //
shimniok 0:a6a169de725f 473 //case MAVLINK_PARSE_STATE_GOT_MSGID:
shimniok 0:a6a169de725f 474 // rxmsg->payload[status->packet_idx++] = c;
shimniok 0:a6a169de725f 475 // mavlink_update_checksum(rxmsg, c);
shimniok 0:a6a169de725f 476 // if (status->packet_idx == rxmsg->len)
shimniok 0:a6a169de725f 477 // {
shimniok 0:a6a169de725f 478 // status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
shimniok 0:a6a169de725f 479 // }
shimniok 0:a6a169de725f 480 // break;
shimniok 0:a6a169de725f 481 //
shimniok 0:a6a169de725f 482 //case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
shimniok 0:a6a169de725f 483 // if (c != rxmsg->ck_a)
shimniok 0:a6a169de725f 484 // {
shimniok 0:a6a169de725f 485 // // Check first checksum byte
shimniok 0:a6a169de725f 486 // status->parse_error++;
shimniok 0:a6a169de725f 487 // status->msg_received = 0;
shimniok 0:a6a169de725f 488 // status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 489 // }
shimniok 0:a6a169de725f 490 // else
shimniok 0:a6a169de725f 491 // {
shimniok 0:a6a169de725f 492 // status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
shimniok 0:a6a169de725f 493 // }
shimniok 0:a6a169de725f 494 // break;
shimniok 0:a6a169de725f 495 //
shimniok 0:a6a169de725f 496 //case MAVLINK_PARSE_STATE_GOT_CRC1:
shimniok 0:a6a169de725f 497 // if (c != rxmsg->ck_b)
shimniok 0:a6a169de725f 498 // {// Check second checksum byte
shimniok 0:a6a169de725f 499 // status->parse_error++;
shimniok 0:a6a169de725f 500 // status->msg_received = 0;
shimniok 0:a6a169de725f 501 // status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 502 // }
shimniok 0:a6a169de725f 503 // else
shimniok 0:a6a169de725f 504 // {
shimniok 0:a6a169de725f 505 // // Successfully got message
shimniok 0:a6a169de725f 506 // status->msg_received = 1;
shimniok 0:a6a169de725f 507 // status->parse_state = MAVLINK_PARSE_STATE_IDLE;
shimniok 0:a6a169de725f 508 // memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
shimniok 0:a6a169de725f 509 // }
shimniok 0:a6a169de725f 510 // break;
shimniok 0:a6a169de725f 511 //}
shimniok 0:a6a169de725f 512
shimniok 0:a6a169de725f 513 bufferIndex++;
shimniok 0:a6a169de725f 514 // If a message has been sucessfully decoded, check index
shimniok 0:a6a169de725f 515 if (status->msg_received == 1)
shimniok 0:a6a169de725f 516 {
shimniok 0:a6a169de725f 517 //while(status->current_seq != rxmsg->seq)
shimniok 0:a6a169de725f 518 //{
shimniok 0:a6a169de725f 519 // status->packet_rx_drop_count++;
shimniok 0:a6a169de725f 520 // status->current_seq++;
shimniok 0:a6a169de725f 521 //}
shimniok 0:a6a169de725f 522 status->current_seq = rxmsg->seq;
shimniok 0:a6a169de725f 523 // Initial condition: If no packet has been received so far, drop count is undefined
shimniok 0:a6a169de725f 524 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
shimniok 0:a6a169de725f 525 // Count this packet as received
shimniok 0:a6a169de725f 526 status->packet_rx_success_count++;
shimniok 0:a6a169de725f 527 }
shimniok 0:a6a169de725f 528
shimniok 0:a6a169de725f 529 r_mavlink_status->current_seq = status->current_seq+1;
shimniok 0:a6a169de725f 530 r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
shimniok 0:a6a169de725f 531 r_mavlink_status->packet_rx_drop_count = status->parse_error;
shimniok 0:a6a169de725f 532 return status->msg_received;
shimniok 0:a6a169de725f 533 }
shimniok 0:a6a169de725f 534 */
shimniok 0:a6a169de725f 535
shimniok 0:a6a169de725f 536
shimniok 0:a6a169de725f 537 typedef union __generic_16bit
shimniok 0:a6a169de725f 538 {
shimniok 0:a6a169de725f 539 uint8_t b[2];
shimniok 0:a6a169de725f 540 int16_t s;
shimniok 0:a6a169de725f 541 } generic_16bit;
shimniok 0:a6a169de725f 542
shimniok 0:a6a169de725f 543 typedef union __generic_32bit
shimniok 0:a6a169de725f 544 {
shimniok 0:a6a169de725f 545 uint8_t b[4];
shimniok 0:a6a169de725f 546 float f;
shimniok 0:a6a169de725f 547 int32_t i;
shimniok 0:a6a169de725f 548 int16_t s;
shimniok 0:a6a169de725f 549 } generic_32bit;
shimniok 0:a6a169de725f 550
shimniok 0:a6a169de725f 551 typedef union __generic_64bit
shimniok 0:a6a169de725f 552 {
shimniok 0:a6a169de725f 553 uint8_t b[8];
shimniok 0:a6a169de725f 554 int64_t ll; ///< Long long (64 bit)
shimniok 0:a6a169de725f 555 double d; ///< IEEE-754 double precision floating point
shimniok 0:a6a169de725f 556 } generic_64bit;
shimniok 0:a6a169de725f 557
shimniok 0:a6a169de725f 558 /**
shimniok 0:a6a169de725f 559 * @brief Place an unsigned byte into the buffer
shimniok 0:a6a169de725f 560 *
shimniok 0:a6a169de725f 561 * @param b the byte to add
shimniok 0:a6a169de725f 562 * @param bindex the position in the packet
shimniok 0:a6a169de725f 563 * @param buffer the packet buffer
shimniok 0:a6a169de725f 564 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 565 */
shimniok 0:a6a169de725f 566 static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 567 {
shimniok 0:a6a169de725f 568 *(buffer + bindex) = b;
shimniok 0:a6a169de725f 569 return sizeof(b);
shimniok 0:a6a169de725f 570 }
shimniok 0:a6a169de725f 571
shimniok 0:a6a169de725f 572 /**
shimniok 0:a6a169de725f 573 * @brief Place a signed byte into the buffer
shimniok 0:a6a169de725f 574 *
shimniok 0:a6a169de725f 575 * @param b the byte to add
shimniok 0:a6a169de725f 576 * @param bindex the position in the packet
shimniok 0:a6a169de725f 577 * @param buffer the packet buffer
shimniok 0:a6a169de725f 578 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 579 */
shimniok 0:a6a169de725f 580 static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 581 {
shimniok 0:a6a169de725f 582 *(buffer + bindex) = (uint8_t)b;
shimniok 0:a6a169de725f 583 return sizeof(b);
shimniok 0:a6a169de725f 584 }
shimniok 0:a6a169de725f 585
shimniok 0:a6a169de725f 586 /**
shimniok 0:a6a169de725f 587 * @brief Place two unsigned bytes into the buffer
shimniok 0:a6a169de725f 588 *
shimniok 0:a6a169de725f 589 * @param b the bytes to add
shimniok 0:a6a169de725f 590 * @param bindex the position in the packet
shimniok 0:a6a169de725f 591 * @param buffer the packet buffer
shimniok 0:a6a169de725f 592 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 593 */
shimniok 0:a6a169de725f 594 static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 595 {
shimniok 0:a6a169de725f 596 buffer[bindex] = (b>>8)&0xff;
shimniok 0:a6a169de725f 597 buffer[bindex+1] = (b & 0xff);
shimniok 0:a6a169de725f 598 return sizeof(b);
shimniok 0:a6a169de725f 599 }
shimniok 0:a6a169de725f 600
shimniok 0:a6a169de725f 601 /**
shimniok 0:a6a169de725f 602 * @brief Place two signed bytes into the buffer
shimniok 0:a6a169de725f 603 *
shimniok 0:a6a169de725f 604 * @param b the bytes to add
shimniok 0:a6a169de725f 605 * @param bindex the position in the packet
shimniok 0:a6a169de725f 606 * @param buffer the packet buffer
shimniok 0:a6a169de725f 607 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 608 */
shimniok 0:a6a169de725f 609 static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 610 {
shimniok 0:a6a169de725f 611 return put_uint16_t_by_index(b, bindex, buffer);
shimniok 0:a6a169de725f 612 }
shimniok 0:a6a169de725f 613
shimniok 0:a6a169de725f 614 /**
shimniok 0:a6a169de725f 615 * @brief Place four unsigned bytes into the buffer
shimniok 0:a6a169de725f 616 *
shimniok 0:a6a169de725f 617 * @param b the bytes to add
shimniok 0:a6a169de725f 618 * @param bindex the position in the packet
shimniok 0:a6a169de725f 619 * @param buffer the packet buffer
shimniok 0:a6a169de725f 620 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 621 */
shimniok 0:a6a169de725f 622 static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 623 {
shimniok 0:a6a169de725f 624 buffer[bindex] = (b>>24)&0xff;
shimniok 0:a6a169de725f 625 buffer[bindex+1] = (b>>16)&0xff;
shimniok 0:a6a169de725f 626 buffer[bindex+2] = (b>>8)&0xff;
shimniok 0:a6a169de725f 627 buffer[bindex+3] = (b & 0xff);
shimniok 0:a6a169de725f 628 return sizeof(b);
shimniok 0:a6a169de725f 629 }
shimniok 0:a6a169de725f 630
shimniok 0:a6a169de725f 631 /**
shimniok 0:a6a169de725f 632 * @brief Place four signed bytes into the buffer
shimniok 0:a6a169de725f 633 *
shimniok 0:a6a169de725f 634 * @param b the bytes to add
shimniok 0:a6a169de725f 635 * @param bindex the position in the packet
shimniok 0:a6a169de725f 636 * @param buffer the packet buffer
shimniok 0:a6a169de725f 637 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 638 */
shimniok 0:a6a169de725f 639 static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 640 {
shimniok 0:a6a169de725f 641 buffer[bindex] = (b>>24)&0xff;
shimniok 0:a6a169de725f 642 buffer[bindex+1] = (b>>16)&0xff;
shimniok 0:a6a169de725f 643 buffer[bindex+2] = (b>>8)&0xff;
shimniok 0:a6a169de725f 644 buffer[bindex+3] = (b & 0xff);
shimniok 0:a6a169de725f 645 return sizeof(b);
shimniok 0:a6a169de725f 646 }
shimniok 0:a6a169de725f 647
shimniok 0:a6a169de725f 648 /**
shimniok 0:a6a169de725f 649 * @brief Place four unsigned bytes into the buffer
shimniok 0:a6a169de725f 650 *
shimniok 0:a6a169de725f 651 * @param b the bytes to add
shimniok 0:a6a169de725f 652 * @param bindex the position in the packet
shimniok 0:a6a169de725f 653 * @param buffer the packet buffer
shimniok 0:a6a169de725f 654 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 655 */
shimniok 0:a6a169de725f 656 static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 657 {
shimniok 0:a6a169de725f 658 buffer[bindex] = (b>>56)&0xff;
shimniok 0:a6a169de725f 659 buffer[bindex+1] = (b>>48)&0xff;
shimniok 0:a6a169de725f 660 buffer[bindex+2] = (b>>40)&0xff;
shimniok 0:a6a169de725f 661 buffer[bindex+3] = (b>>32)&0xff;
shimniok 0:a6a169de725f 662 buffer[bindex+4] = (b>>24)&0xff;
shimniok 0:a6a169de725f 663 buffer[bindex+5] = (b>>16)&0xff;
shimniok 0:a6a169de725f 664 buffer[bindex+6] = (b>>8)&0xff;
shimniok 0:a6a169de725f 665 buffer[bindex+7] = (b & 0xff);
shimniok 0:a6a169de725f 666 return sizeof(b);
shimniok 0:a6a169de725f 667 }
shimniok 0:a6a169de725f 668
shimniok 0:a6a169de725f 669 /**
shimniok 0:a6a169de725f 670 * @brief Place four signed bytes into the buffer
shimniok 0:a6a169de725f 671 *
shimniok 0:a6a169de725f 672 * @param b the bytes to add
shimniok 0:a6a169de725f 673 * @param bindex the position in the packet
shimniok 0:a6a169de725f 674 * @param buffer the packet buffer
shimniok 0:a6a169de725f 675 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 676 */
shimniok 0:a6a169de725f 677 static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 678 {
shimniok 0:a6a169de725f 679 return put_uint64_t_by_index(b, bindex, buffer);
shimniok 0:a6a169de725f 680 }
shimniok 0:a6a169de725f 681
shimniok 0:a6a169de725f 682 /**
shimniok 0:a6a169de725f 683 * @brief Place a float into the buffer
shimniok 0:a6a169de725f 684 *
shimniok 0:a6a169de725f 685 * @param b the float to add
shimniok 0:a6a169de725f 686 * @param bindex the position in the packet
shimniok 0:a6a169de725f 687 * @param buffer the packet buffer
shimniok 0:a6a169de725f 688 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 689 */
shimniok 0:a6a169de725f 690 static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 691 {
shimniok 0:a6a169de725f 692 generic_32bit g;
shimniok 0:a6a169de725f 693 g.f = b;
shimniok 0:a6a169de725f 694 return put_int32_t_by_index(g.i, bindex, buffer);
shimniok 0:a6a169de725f 695 }
shimniok 0:a6a169de725f 696
shimniok 0:a6a169de725f 697 /**
shimniok 0:a6a169de725f 698 * @brief Place a double into the buffer
shimniok 0:a6a169de725f 699 *
shimniok 0:a6a169de725f 700 * @param b the double to add
shimniok 0:a6a169de725f 701 * @param bindex the position in the packet
shimniok 0:a6a169de725f 702 * @param buffer the packet buffer
shimniok 0:a6a169de725f 703 * @return the new position of the last used byte in the buffer
shimniok 0:a6a169de725f 704 */
shimniok 0:a6a169de725f 705 static inline uint8_t put_double_by_index(double b, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 706 {
shimniok 0:a6a169de725f 707 generic_64bit g;
shimniok 0:a6a169de725f 708 g.d = b;
shimniok 0:a6a169de725f 709 return put_int64_t_by_index(g.ll, bindex, buffer);
shimniok 0:a6a169de725f 710 }
shimniok 0:a6a169de725f 711
shimniok 0:a6a169de725f 712 /**
shimniok 0:a6a169de725f 713 * @brief Place an array into the buffer
shimniok 0:a6a169de725f 714 *
shimniok 0:a6a169de725f 715 * @param b the array to add
shimniok 0:a6a169de725f 716 * @param length size of the array (for strings: length WITH '\0' char)
shimniok 0:a6a169de725f 717 * @param bindex the position in the packet
shimniok 0:a6a169de725f 718 * @param buffer packet buffer
shimniok 0:a6a169de725f 719 * @return new position of the last used byte in the buffer
shimniok 0:a6a169de725f 720 */
shimniok 0:a6a169de725f 721 static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 722 {
shimniok 0:a6a169de725f 723 memcpy(buffer+bindex, b, length);
shimniok 0:a6a169de725f 724 return length;
shimniok 0:a6a169de725f 725 }
shimniok 0:a6a169de725f 726
shimniok 0:a6a169de725f 727 /**
shimniok 0:a6a169de725f 728 * @brief Place a string into the buffer
shimniok 0:a6a169de725f 729 *
shimniok 0:a6a169de725f 730 * @param b the string to add
shimniok 0:a6a169de725f 731 * @param maxlength size of the array (for strings: length WITHOUT '\0' char)
shimniok 0:a6a169de725f 732 * @param bindex the position in the packet
shimniok 0:a6a169de725f 733 * @param buffer packet buffer
shimniok 0:a6a169de725f 734 * @return new position of the last used byte in the buffer
shimniok 0:a6a169de725f 735 */
shimniok 0:a6a169de725f 736 static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer)
shimniok 0:a6a169de725f 737 {
shimniok 0:a6a169de725f 738 uint16_t length = 0;
shimniok 0:a6a169de725f 739 // Copy string into buffer, ensuring not to exceed the buffer size
shimniok 0:a6a169de725f 740 int i;
shimniok 0:a6a169de725f 741 for (i = 1; i < maxlength; i++)
shimniok 0:a6a169de725f 742 {
shimniok 0:a6a169de725f 743 length++;
shimniok 0:a6a169de725f 744 // String characters
shimniok 0:a6a169de725f 745 if (i < (maxlength - 1))
shimniok 0:a6a169de725f 746 {
shimniok 0:a6a169de725f 747 buffer[bindex+i] = b[i];
shimniok 0:a6a169de725f 748 // Stop at null character
shimniok 0:a6a169de725f 749 if (b[i] == '\0')
shimniok 0:a6a169de725f 750 {
shimniok 0:a6a169de725f 751 break;
shimniok 0:a6a169de725f 752 }
shimniok 0:a6a169de725f 753 }
shimniok 0:a6a169de725f 754 // Enforce null termination at end of buffer
shimniok 0:a6a169de725f 755 else if (i == (maxlength - 1))
shimniok 0:a6a169de725f 756 {
shimniok 0:a6a169de725f 757 buffer[i] = '\0';
shimniok 0:a6a169de725f 758 }
shimniok 0:a6a169de725f 759 }
shimniok 0:a6a169de725f 760 // Write length into first field
shimniok 0:a6a169de725f 761 put_uint8_t_by_index(length, bindex, buffer);
shimniok 0:a6a169de725f 762 return length;
shimniok 0:a6a169de725f 763 }
shimniok 0:a6a169de725f 764
shimniok 0:a6a169de725f 765 /**
shimniok 0:a6a169de725f 766 * @brief Put a bitfield of length 1-32 bit into the buffer
shimniok 0:a6a169de725f 767 *
shimniok 0:a6a169de725f 768 * @param b the value to add, will be encoded in the bitfield
shimniok 0:a6a169de725f 769 * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
shimniok 0:a6a169de725f 770 * @param packet_index the position in the packet (the index of the first byte to use)
shimniok 0:a6a169de725f 771 * @param bit_index the position in the byte (the index of the first bit to use)
shimniok 0:a6a169de725f 772 * @param buffer packet buffer to write into
shimniok 0:a6a169de725f 773 * @return new position of the last used byte in the buffer
shimniok 0:a6a169de725f 774 */
shimniok 0:a6a169de725f 775 static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
shimniok 0:a6a169de725f 776 {
shimniok 0:a6a169de725f 777 uint16_t bits_remain = bits;
shimniok 0:a6a169de725f 778 // Transform number into network order
shimniok 0:a6a169de725f 779 generic_32bit bin;
shimniok 0:a6a169de725f 780 generic_32bit bout;
shimniok 0:a6a169de725f 781 uint8_t i_bit_index, i_byte_index, curr_bits_n;
shimniok 0:a6a169de725f 782 bin.i = b;
shimniok 0:a6a169de725f 783 bout.b[0] = bin.b[3];
shimniok 0:a6a169de725f 784 bout.b[1] = bin.b[2];
shimniok 0:a6a169de725f 785 bout.b[2] = bin.b[1];
shimniok 0:a6a169de725f 786 bout.b[3] = bin.b[0];
shimniok 0:a6a169de725f 787
shimniok 0:a6a169de725f 788 // buffer in
shimniok 0:a6a169de725f 789 // 01100000 01000000 00000000 11110001
shimniok 0:a6a169de725f 790 // buffer out
shimniok 0:a6a169de725f 791 // 11110001 00000000 01000000 01100000
shimniok 0:a6a169de725f 792
shimniok 0:a6a169de725f 793 // Existing partly filled byte (four free slots)
shimniok 0:a6a169de725f 794 // 0111xxxx
shimniok 0:a6a169de725f 795
shimniok 0:a6a169de725f 796 // Mask n free bits
shimniok 0:a6a169de725f 797 // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
shimniok 0:a6a169de725f 798 // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
shimniok 0:a6a169de725f 799
shimniok 0:a6a169de725f 800 // Shift n bits into the right position
shimniok 0:a6a169de725f 801 // out = in >> n;
shimniok 0:a6a169de725f 802
shimniok 0:a6a169de725f 803 // Mask and shift bytes
shimniok 0:a6a169de725f 804 i_bit_index = bit_index;
shimniok 0:a6a169de725f 805 i_byte_index = packet_index;
shimniok 0:a6a169de725f 806 if (bit_index > 0)
shimniok 0:a6a169de725f 807 {
shimniok 0:a6a169de725f 808 // If bits were available at start, they were available
shimniok 0:a6a169de725f 809 // in the byte before the current index
shimniok 0:a6a169de725f 810 i_byte_index--;
shimniok 0:a6a169de725f 811 }
shimniok 0:a6a169de725f 812
shimniok 0:a6a169de725f 813 // While bits have not been packed yet
shimniok 0:a6a169de725f 814 while (bits_remain > 0)
shimniok 0:a6a169de725f 815 {
shimniok 0:a6a169de725f 816 // Bits still have to be packed
shimniok 0:a6a169de725f 817 // there can be more than 8 bits, so
shimniok 0:a6a169de725f 818 // we might have to pack them into more than one byte
shimniok 0:a6a169de725f 819
shimniok 0:a6a169de725f 820 // First pack everything we can into the current 'open' byte
shimniok 0:a6a169de725f 821 //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
shimniok 0:a6a169de725f 822 //FIXME
shimniok 0:a6a169de725f 823 if (bits_remain <= (8 - i_bit_index))
shimniok 0:a6a169de725f 824 {
shimniok 0:a6a169de725f 825 // Enough space
shimniok 0:a6a169de725f 826 curr_bits_n = bits_remain;
shimniok 0:a6a169de725f 827 }
shimniok 0:a6a169de725f 828 else
shimniok 0:a6a169de725f 829 {
shimniok 0:a6a169de725f 830 curr_bits_n = (8 - i_bit_index);
shimniok 0:a6a169de725f 831 }
shimniok 0:a6a169de725f 832
shimniok 0:a6a169de725f 833 // Pack these n bits into the current byte
shimniok 0:a6a169de725f 834 // Mask out whatever was at that position with ones (xxx11111)
shimniok 0:a6a169de725f 835 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
shimniok 0:a6a169de725f 836 // Put content to this position, by masking out the non-used part
shimniok 0:a6a169de725f 837 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i);
shimniok 0:a6a169de725f 838
shimniok 0:a6a169de725f 839 // Increment the bit index
shimniok 0:a6a169de725f 840 i_bit_index += curr_bits_n;
shimniok 0:a6a169de725f 841
shimniok 0:a6a169de725f 842 // Now proceed to the next byte, if necessary
shimniok 0:a6a169de725f 843 bits_remain -= curr_bits_n;
shimniok 0:a6a169de725f 844 if (bits_remain > 0)
shimniok 0:a6a169de725f 845 {
shimniok 0:a6a169de725f 846 // Offer another 8 bits / one byte
shimniok 0:a6a169de725f 847 i_byte_index++;
shimniok 0:a6a169de725f 848 i_bit_index = 0;
shimniok 0:a6a169de725f 849 }
shimniok 0:a6a169de725f 850 }
shimniok 0:a6a169de725f 851
shimniok 0:a6a169de725f 852 *r_bit_index = i_bit_index;
shimniok 0:a6a169de725f 853 // If a partly filled byte is present, mark this as consumed
shimniok 0:a6a169de725f 854 if (i_bit_index != 7) i_byte_index++;
shimniok 0:a6a169de725f 855 return i_byte_index - packet_index;
shimniok 0:a6a169de725f 856 }
shimniok 0:a6a169de725f 857
shimniok 0:a6a169de725f 858 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
shimniok 0:a6a169de725f 859
shimniok 0:a6a169de725f 860 // To make MAVLink work on your MCU, define a similar function
shimniok 0:a6a169de725f 861
shimniok 0:a6a169de725f 862 /*
shimniok 0:a6a169de725f 863
shimniok 0:a6a169de725f 864 #include "mavlink_types.h"
shimniok 0:a6a169de725f 865
shimniok 0:a6a169de725f 866 void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
shimniok 0:a6a169de725f 867 {
shimniok 0:a6a169de725f 868 if (chan == MAVLINK_COMM_0)
shimniok 0:a6a169de725f 869 {
shimniok 0:a6a169de725f 870 uart0_transmit(ch);
shimniok 0:a6a169de725f 871 }
shimniok 0:a6a169de725f 872 if (chan == MAVLINK_COMM_1)
shimniok 0:a6a169de725f 873 {
shimniok 0:a6a169de725f 874 uart1_transmit(ch);
shimniok 0:a6a169de725f 875 }
shimniok 0:a6a169de725f 876 }
shimniok 0:a6a169de725f 877 */
shimniok 0:a6a169de725f 878
shimniok 0:a6a169de725f 879 static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 880 {
shimniok 0:a6a169de725f 881 crc_accumulate(b, checksum);
shimniok 0:a6a169de725f 882 comm_send_ch(chan, b);
shimniok 0:a6a169de725f 883 }
shimniok 0:a6a169de725f 884
shimniok 0:a6a169de725f 885 static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 886 {
shimniok 0:a6a169de725f 887 crc_accumulate(b, checksum);
shimniok 0:a6a169de725f 888 comm_send_ch(chan, b);
shimniok 0:a6a169de725f 889 }
shimniok 0:a6a169de725f 890
shimniok 0:a6a169de725f 891 static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 892 {
shimniok 0:a6a169de725f 893 char s;
shimniok 0:a6a169de725f 894 s = (b>>8)&0xff;
shimniok 0:a6a169de725f 895 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 896 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 897 s = (b & 0xff);
shimniok 0:a6a169de725f 898 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 899 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 900 }
shimniok 0:a6a169de725f 901
shimniok 0:a6a169de725f 902 static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 903 {
shimniok 0:a6a169de725f 904 mavlink_send_uart_uint16_t(chan, b, checksum);
shimniok 0:a6a169de725f 905 }
shimniok 0:a6a169de725f 906
shimniok 0:a6a169de725f 907 static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 908 {
shimniok 0:a6a169de725f 909 char s;
shimniok 0:a6a169de725f 910 s = (b>>24)&0xff;
shimniok 0:a6a169de725f 911 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 912 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 913 s = (b>>16)&0xff;
shimniok 0:a6a169de725f 914 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 915 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 916 s = (b>>8)&0xff;
shimniok 0:a6a169de725f 917 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 918 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 919 s = (b & 0xff);
shimniok 0:a6a169de725f 920 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 921 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 922 }
shimniok 0:a6a169de725f 923
shimniok 0:a6a169de725f 924 static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 925 {
shimniok 0:a6a169de725f 926 mavlink_send_uart_uint32_t(chan, b, checksum);
shimniok 0:a6a169de725f 927 }
shimniok 0:a6a169de725f 928
shimniok 0:a6a169de725f 929 static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 930 {
shimniok 0:a6a169de725f 931 char s;
shimniok 0:a6a169de725f 932 s = (b>>56)&0xff;
shimniok 0:a6a169de725f 933 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 934 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 935 s = (b>>48)&0xff;
shimniok 0:a6a169de725f 936 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 937 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 938 s = (b>>40)&0xff;
shimniok 0:a6a169de725f 939 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 940 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 941 s = (b>>32)&0xff;
shimniok 0:a6a169de725f 942 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 943 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 944 s = (b>>24)&0xff;
shimniok 0:a6a169de725f 945 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 946 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 947 s = (b>>16)&0xff;
shimniok 0:a6a169de725f 948 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 949 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 950 s = (b>>8)&0xff;
shimniok 0:a6a169de725f 951 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 952 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 953 s = (b & 0xff);
shimniok 0:a6a169de725f 954 comm_send_ch(chan, s);
shimniok 0:a6a169de725f 955 crc_accumulate(s, checksum);
shimniok 0:a6a169de725f 956 }
shimniok 0:a6a169de725f 957
shimniok 0:a6a169de725f 958 static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum)
shimniok 0:a6a169de725f 959 {
shimniok 0:a6a169de725f 960 mavlink_send_uart_uint64_t(chan, b, checksum);
shimniok 0:a6a169de725f 961 }
shimniok 0:a6a169de725f 962
shimniok 0:a6a169de725f 963 static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum)
shimniok 0:a6a169de725f 964 {
shimniok 0:a6a169de725f 965 generic_32bit g;
shimniok 0:a6a169de725f 966 g.f = b;
shimniok 0:a6a169de725f 967 mavlink_send_uart_uint32_t(chan, g.i, checksum);
shimniok 0:a6a169de725f 968 }
shimniok 0:a6a169de725f 969
shimniok 0:a6a169de725f 970 static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum)
shimniok 0:a6a169de725f 971 {
shimniok 0:a6a169de725f 972 generic_64bit g;
shimniok 0:a6a169de725f 973 g.d = b;
shimniok 0:a6a169de725f 974 mavlink_send_uart_uint32_t(chan, g.ll, checksum);
shimniok 0:a6a169de725f 975 }
shimniok 0:a6a169de725f 976
shimniok 0:a6a169de725f 977 static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg)
shimniok 0:a6a169de725f 978 {
shimniok 0:a6a169de725f 979 // ARM7 MCU board implementation
shimniok 0:a6a169de725f 980 // Create pointer on message struct
shimniok 0:a6a169de725f 981 // Send STX
shimniok 0:a6a169de725f 982 comm_send_ch(chan, MAVLINK_STX);
shimniok 0:a6a169de725f 983 comm_send_ch(chan, msg->len);
shimniok 0:a6a169de725f 984 comm_send_ch(chan, msg->seq);
shimniok 0:a6a169de725f 985 comm_send_ch(chan, msg->sysid);
shimniok 0:a6a169de725f 986 comm_send_ch(chan, msg->compid);
shimniok 0:a6a169de725f 987 comm_send_ch(chan, msg->msgid);
shimniok 0:a6a169de725f 988 for(uint16_t i = 0; i < msg->len; i++)
shimniok 0:a6a169de725f 989 {
shimniok 0:a6a169de725f 990 comm_send_ch(chan, msg->payload[i]);
shimniok 0:a6a169de725f 991 }
shimniok 0:a6a169de725f 992 comm_send_ch(chan, msg->ck_a);
shimniok 0:a6a169de725f 993 comm_send_ch(chan, msg->ck_b);
shimniok 0:a6a169de725f 994 }
shimniok 0:a6a169de725f 995 #endif
shimniok 0:a6a169de725f 996
shimniok 0:a6a169de725f 997 #endif /* _MAVLINK_PROTOCOL_H_ */