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@36:1bbd2fb7d2c8, 2014-03-19 (annotated)

Committer:
Gaetan
Date:
Wed Mar 19 11:01:10 2014 +0000
Revision:
36:1bbd2fb7d2c8
Parent:
35:95cb34636703 
Checksum correct pour l'envoi de n'importe quel message Mavlink. Reste ? g?rer la reception de messages

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