Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

MAVlink/include/protocol.h@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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