HERBERT Nicolas / Mbed 2 deprecated Labo_TRSE_Drone

Projet Drone de surveillance du labo TRSE (INGESUP)

Dependencies:   mbed PID ADXL345 Camera_LS_Y201 ITG3200 RangeFinder mbos xbee_lib Motor Servo

Module_Communication/MAVlink/include/protocol.h@35:95cb34636703, 2014-03-19 (annotated)

Committer:
Gaetan
Date:
Wed Mar 19 09:27:19 2014 +0000
Revision:
35:95cb34636703
Child:
36:1bbd2fb7d2c8
Ajout de la librairie MavLink avec un main de test pour QGroundControl.; Pour l'instant seul le heartbeat fonctionne;

Who changed what in which revision?

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