[88] | 1 | #ifndef _MAVLINK_HELPERS_H_
|
---|
| 2 | #define _MAVLINK_HELPERS_H_
|
---|
| 3 |
|
---|
| 4 | #include "string.h"
|
---|
| 5 | #include "checksum.h"
|
---|
| 6 | #include "mavlink_types.h"
|
---|
| 7 | #include "mavlink_conversions.h"
|
---|
| 8 |
|
---|
| 9 | #ifndef MAVLINK_HELPER
|
---|
| 10 | #define MAVLINK_HELPER
|
---|
| 11 | #endif
|
---|
| 12 |
|
---|
| 13 | /*
|
---|
| 14 | * Internal function to give access to the channel status for each channel
|
---|
| 15 | */
|
---|
| 16 | #ifndef MAVLINK_GET_CHANNEL_STATUS
|
---|
| 17 | MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
|
---|
| 18 | {
|
---|
| 19 | #ifdef MAVLINK_EXTERNAL_RX_STATUS
|
---|
| 20 | // No m_mavlink_status array defined in function,
|
---|
| 21 | // has to be defined externally
|
---|
| 22 | #else
|
---|
| 23 | static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
---|
| 24 | #endif
|
---|
| 25 | return &m_mavlink_status[chan];
|
---|
| 26 | }
|
---|
| 27 | #endif
|
---|
| 28 |
|
---|
| 29 | /*
|
---|
| 30 | * Internal function to give access to the channel buffer for each channel
|
---|
| 31 | */
|
---|
| 32 | #ifndef MAVLINK_GET_CHANNEL_BUFFER
|
---|
| 33 | MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
|
---|
| 34 | {
|
---|
| 35 |
|
---|
| 36 | #ifdef MAVLINK_EXTERNAL_RX_BUFFER
|
---|
| 37 | // No m_mavlink_buffer array defined in function,
|
---|
| 38 | // has to be defined externally
|
---|
| 39 | #else
|
---|
| 40 | static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
|
---|
| 41 | #endif
|
---|
| 42 | return &m_mavlink_buffer[chan];
|
---|
| 43 | }
|
---|
| 44 | #endif
|
---|
| 45 |
|
---|
| 46 | /**
|
---|
| 47 | * @brief Reset the status of a channel.
|
---|
| 48 | */
|
---|
| 49 | MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
|
---|
| 50 | {
|
---|
| 51 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 52 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 53 | }
|
---|
| 54 |
|
---|
| 55 | /**
|
---|
| 56 | * @brief Finalize a MAVLink message with channel assignment
|
---|
| 57 | *
|
---|
| 58 | * This function calculates the checksum and sets length and aircraft id correctly.
|
---|
| 59 | * It assumes that the message id and the payload are already correctly set. This function
|
---|
| 60 | * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
|
---|
| 61 | * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
|
---|
| 62 | *
|
---|
| 63 | * @param msg Message to finalize
|
---|
| 64 | * @param system_id Id of the sending (this) system, 1-127
|
---|
| 65 | * @param length Message length
|
---|
| 66 | */
|
---|
| 67 | #if MAVLINK_CRC_EXTRA
|
---|
| 68 | MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
---|
| 69 | uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
---|
| 70 | #else
|
---|
| 71 | MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
---|
| 72 | uint8_t chan, uint8_t length)
|
---|
| 73 | #endif
|
---|
| 74 | {
|
---|
| 75 | // This code part is the same for all messages;
|
---|
| 76 | msg->magic = MAVLINK_STX;
|
---|
| 77 | msg->len = length;
|
---|
| 78 | msg->sysid = system_id;
|
---|
| 79 | msg->compid = component_id;
|
---|
| 80 | // One sequence number per channel
|
---|
| 81 | msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
|
---|
| 82 | mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
|
---|
| 83 | msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
|
---|
| 84 | crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
|
---|
| 85 | #if MAVLINK_CRC_EXTRA
|
---|
| 86 | crc_accumulate(crc_extra, &msg->checksum);
|
---|
| 87 | #endif
|
---|
| 88 | mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
|
---|
| 89 | mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
|
---|
| 90 |
|
---|
| 91 | return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
---|
| 92 | }
|
---|
| 93 |
|
---|
| 94 |
|
---|
| 95 | /**
|
---|
| 96 | * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
|
---|
| 97 | */
|
---|
| 98 | #if MAVLINK_CRC_EXTRA
|
---|
| 99 | MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
---|
| 100 | uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
---|
| 101 | {
|
---|
| 102 | return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
|
---|
| 103 | }
|
---|
| 104 | #else
|
---|
| 105 | MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
---|
| 106 | uint8_t length)
|
---|
| 107 | {
|
---|
| 108 | return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
|
---|
| 109 | }
|
---|
| 110 | #endif
|
---|
| 111 |
|
---|
| 112 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 113 | MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
|
---|
| 114 |
|
---|
| 115 | /**
|
---|
| 116 | * @brief Finalize a MAVLink message with channel assignment and send
|
---|
| 117 | */
|
---|
| 118 | #if MAVLINK_CRC_EXTRA
|
---|
| 119 | MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
|
---|
| 120 | uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
---|
| 121 | #else
|
---|
| 122 | MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
|
---|
| 123 | #endif
|
---|
| 124 | {
|
---|
| 125 | uint16_t checksum;
|
---|
| 126 | uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
|
---|
| 127 | uint8_t ck[2];
|
---|
| 128 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 129 | buf[0] = MAVLINK_STX;
|
---|
| 130 | buf[1] = length;
|
---|
| 131 | buf[2] = status->current_tx_seq;
|
---|
| 132 | buf[3] = mavlink_system.sysid;
|
---|
| 133 | buf[4] = mavlink_system.compid;
|
---|
| 134 | buf[5] = msgid;
|
---|
| 135 | status->current_tx_seq++;
|
---|
| 136 | checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
|
---|
| 137 | crc_accumulate_buffer(&checksum, packet, length);
|
---|
| 138 | #if MAVLINK_CRC_EXTRA
|
---|
| 139 | crc_accumulate(crc_extra, &checksum);
|
---|
| 140 | #endif
|
---|
| 141 | ck[0] = (uint8_t)(checksum & 0xFF);
|
---|
| 142 | ck[1] = (uint8_t)(checksum >> 8);
|
---|
| 143 |
|
---|
| 144 | MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
|
---|
| 145 | _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
|
---|
| 146 | _mavlink_send_uart(chan, packet, length);
|
---|
| 147 | _mavlink_send_uart(chan, (const char *)ck, 2);
|
---|
| 148 | MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
|
---|
| 149 | }
|
---|
| 150 |
|
---|
| 151 | /**
|
---|
| 152 | * @brief re-send a message over a uart channel
|
---|
| 153 | * this is more stack efficient than re-marshalling the message
|
---|
| 154 | */
|
---|
| 155 | MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
|
---|
| 156 | {
|
---|
| 157 | uint8_t ck[2];
|
---|
| 158 |
|
---|
| 159 | ck[0] = (uint8_t)(msg->checksum & 0xFF);
|
---|
| 160 | ck[1] = (uint8_t)(msg->checksum >> 8);
|
---|
| 161 | // XXX use the right sequence here
|
---|
| 162 |
|
---|
| 163 | MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
|
---|
| 164 | _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
|
---|
| 165 | _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
|
---|
| 166 | _mavlink_send_uart(chan, (const char *)ck, 2);
|
---|
| 167 | MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
|
---|
| 168 | }
|
---|
| 169 | #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 170 |
|
---|
| 171 | /**
|
---|
| 172 | * @brief Pack a message to send it over a serial byte stream
|
---|
| 173 | */
|
---|
| 174 | MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
|
---|
| 175 | {
|
---|
| 176 | memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
|
---|
| 177 |
|
---|
| 178 | uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
|
---|
| 179 |
|
---|
| 180 | ck[0] = (uint8_t)(msg->checksum & 0xFF);
|
---|
| 181 | ck[1] = (uint8_t)(msg->checksum >> 8);
|
---|
| 182 |
|
---|
| 183 | return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
|
---|
| 184 | }
|
---|
| 185 |
|
---|
| 186 | union __mavlink_bitfield {
|
---|
| 187 | uint8_t uint8;
|
---|
| 188 | int8_t int8;
|
---|
| 189 | uint16_t uint16;
|
---|
| 190 | int16_t int16;
|
---|
| 191 | uint32_t uint32;
|
---|
| 192 | int32_t int32;
|
---|
| 193 | };
|
---|
| 194 |
|
---|
| 195 |
|
---|
| 196 | MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
|
---|
| 197 | {
|
---|
| 198 | crc_init(&msg->checksum);
|
---|
| 199 | }
|
---|
| 200 |
|
---|
| 201 | MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
|
---|
| 202 | {
|
---|
| 203 | crc_accumulate(c, &msg->checksum);
|
---|
| 204 | }
|
---|
| 205 |
|
---|
| 206 | /**
|
---|
| 207 | * This is a varient of mavlink_frame_char() but with caller supplied
|
---|
| 208 | * parsing buffers. It is useful when you want to create a MAVLink
|
---|
| 209 | * parser in a library that doesn't use any global variables
|
---|
| 210 | *
|
---|
| 211 | * @param rxmsg parsing message buffer
|
---|
| 212 | * @param status parsing starus buffer
|
---|
| 213 | * @param c The char to parse
|
---|
| 214 | *
|
---|
| 215 | * @param returnMsg NULL if no message could be decoded, the message data else
|
---|
| 216 | * @param returnStats if a message was decoded, this is filled with the channel's stats
|
---|
| 217 | * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
|
---|
| 218 | *
|
---|
| 219 | * A typical use scenario of this function call is:
|
---|
| 220 | *
|
---|
| 221 | * @code
|
---|
| 222 | * #include <mavlink.h>
|
---|
| 223 | *
|
---|
| 224 | * mavlink_message_t msg;
|
---|
| 225 | * int chan = 0;
|
---|
| 226 | *
|
---|
| 227 | *
|
---|
| 228 | * while(serial.bytesAvailable > 0)
|
---|
| 229 | * {
|
---|
| 230 | * uint8_t byte = serial.getNextByte();
|
---|
| 231 | * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
|
---|
| 232 | * {
|
---|
| 233 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
|
---|
| 234 | * }
|
---|
| 235 | * }
|
---|
| 236 | *
|
---|
| 237 | *
|
---|
| 238 | * @endcode
|
---|
| 239 | */
|
---|
| 240 | MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
---|
| 241 | mavlink_status_t* status,
|
---|
| 242 | uint8_t c,
|
---|
| 243 | mavlink_message_t* r_message,
|
---|
| 244 | mavlink_status_t* r_mavlink_status)
|
---|
| 245 | {
|
---|
| 246 | /*
|
---|
| 247 | default message crc function. You can override this per-system to
|
---|
| 248 | put this data in a different memory segment
|
---|
| 249 | */
|
---|
| 250 | #if MAVLINK_CRC_EXTRA
|
---|
| 251 | #ifndef MAVLINK_MESSAGE_CRC
|
---|
| 252 | static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
|
---|
| 253 | #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
|
---|
| 254 | #endif
|
---|
| 255 | #endif
|
---|
| 256 |
|
---|
| 257 | /* Enable this option to check the length of each message.
|
---|
| 258 | This allows invalid messages to be caught much sooner. Use if the transmission
|
---|
| 259 | medium is prone to missing (or extra) characters (e.g. a radio that fades in
|
---|
| 260 | and out). Only use if the channel will only contain messages types listed in
|
---|
| 261 | the headers.
|
---|
| 262 | */
|
---|
| 263 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
---|
| 264 | #ifndef MAVLINK_MESSAGE_LENGTH
|
---|
| 265 | static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
---|
| 266 | #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
|
---|
| 267 | #endif
|
---|
| 268 | #endif
|
---|
| 269 |
|
---|
| 270 | int bufferIndex = 0;
|
---|
| 271 |
|
---|
| 272 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
---|
| 273 |
|
---|
| 274 | switch (status->parse_state)
|
---|
| 275 | {
|
---|
| 276 | case MAVLINK_PARSE_STATE_UNINIT:
|
---|
| 277 | case MAVLINK_PARSE_STATE_IDLE:
|
---|
| 278 | if (c == MAVLINK_STX)
|
---|
| 279 | {
|
---|
| 280 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
---|
| 281 | rxmsg->len = 0;
|
---|
| 282 | rxmsg->magic = c;
|
---|
| 283 | mavlink_start_checksum(rxmsg);
|
---|
| 284 | }
|
---|
| 285 | break;
|
---|
| 286 |
|
---|
| 287 | case MAVLINK_PARSE_STATE_GOT_STX:
|
---|
| 288 | if (status->msg_received
|
---|
| 289 | /* Support shorter buffers than the
|
---|
| 290 | default maximum packet size */
|
---|
| 291 | #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
|
---|
| 292 | || c > MAVLINK_MAX_PAYLOAD_LEN
|
---|
| 293 | #endif
|
---|
| 294 | )
|
---|
| 295 | {
|
---|
| 296 | status->buffer_overrun++;
|
---|
| 297 | status->parse_error++;
|
---|
| 298 | status->msg_received = 0;
|
---|
| 299 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 300 | }
|
---|
| 301 | else
|
---|
| 302 | {
|
---|
| 303 | // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
|
---|
| 304 | rxmsg->len = c;
|
---|
| 305 | status->packet_idx = 0;
|
---|
| 306 | mavlink_update_checksum(rxmsg, c);
|
---|
| 307 | status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
|
---|
| 308 | }
|
---|
| 309 | break;
|
---|
| 310 |
|
---|
| 311 | case MAVLINK_PARSE_STATE_GOT_LENGTH:
|
---|
| 312 | rxmsg->seq = c;
|
---|
| 313 | mavlink_update_checksum(rxmsg, c);
|
---|
| 314 | status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
|
---|
| 315 | break;
|
---|
| 316 |
|
---|
| 317 | case MAVLINK_PARSE_STATE_GOT_SEQ:
|
---|
| 318 | rxmsg->sysid = c;
|
---|
| 319 | mavlink_update_checksum(rxmsg, c);
|
---|
| 320 | status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
|
---|
| 321 | break;
|
---|
| 322 |
|
---|
| 323 | case MAVLINK_PARSE_STATE_GOT_SYSID:
|
---|
| 324 | rxmsg->compid = c;
|
---|
| 325 | mavlink_update_checksum(rxmsg, c);
|
---|
| 326 | status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
|
---|
| 327 | break;
|
---|
| 328 |
|
---|
| 329 | case MAVLINK_PARSE_STATE_GOT_COMPID:
|
---|
| 330 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
---|
| 331 | if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
|
---|
| 332 | {
|
---|
| 333 | status->parse_error++;
|
---|
| 334 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 335 | break;
|
---|
| 336 | }
|
---|
| 337 | #endif
|
---|
| 338 | rxmsg->msgid = c;
|
---|
| 339 | mavlink_update_checksum(rxmsg, c);
|
---|
| 340 | if (rxmsg->len == 0)
|
---|
| 341 | {
|
---|
| 342 | status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
|
---|
| 343 | }
|
---|
| 344 | else
|
---|
| 345 | {
|
---|
| 346 | status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
|
---|
| 347 | }
|
---|
| 348 | break;
|
---|
| 349 |
|
---|
| 350 | case MAVLINK_PARSE_STATE_GOT_MSGID:
|
---|
| 351 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
|
---|
| 352 | mavlink_update_checksum(rxmsg, c);
|
---|
| 353 | if (status->packet_idx == rxmsg->len)
|
---|
| 354 | {
|
---|
| 355 | status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
|
---|
| 356 | }
|
---|
| 357 | break;
|
---|
| 358 |
|
---|
| 359 | case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
|
---|
| 360 | #if MAVLINK_CRC_EXTRA
|
---|
| 361 | mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
|
---|
| 362 | #endif
|
---|
| 363 | if (c != (rxmsg->checksum & 0xFF)) {
|
---|
| 364 | status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
|
---|
| 365 | } else {
|
---|
| 366 | status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
|
---|
| 367 | }
|
---|
| 368 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
|
---|
| 369 | break;
|
---|
| 370 |
|
---|
| 371 | case MAVLINK_PARSE_STATE_GOT_CRC1:
|
---|
| 372 | case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
|
---|
| 373 | if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
|
---|
| 374 | // got a bad CRC message
|
---|
| 375 | status->msg_received = MAVLINK_FRAMING_BAD_CRC;
|
---|
| 376 | } else {
|
---|
| 377 | // Successfully got message
|
---|
| 378 | status->msg_received = MAVLINK_FRAMING_OK;
|
---|
| 379 | }
|
---|
| 380 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 381 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
|
---|
| 382 | memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
|
---|
| 383 | break;
|
---|
| 384 | }
|
---|
| 385 |
|
---|
| 386 | bufferIndex++;
|
---|
| 387 | // If a message has been sucessfully decoded, check index
|
---|
| 388 | if (status->msg_received == MAVLINK_FRAMING_OK)
|
---|
| 389 | {
|
---|
| 390 | //while(status->current_seq != rxmsg->seq)
|
---|
| 391 | //{
|
---|
| 392 | // status->packet_rx_drop_count++;
|
---|
| 393 | // status->current_seq++;
|
---|
| 394 | //}
|
---|
| 395 | status->current_rx_seq = rxmsg->seq;
|
---|
| 396 | // Initial condition: If no packet has been received so far, drop count is undefined
|
---|
| 397 | if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
|
---|
| 398 | // Count this packet as received
|
---|
| 399 | status->packet_rx_success_count++;
|
---|
| 400 | }
|
---|
| 401 |
|
---|
| 402 | r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
|
---|
| 403 | r_mavlink_status->parse_state = status->parse_state;
|
---|
| 404 | r_mavlink_status->packet_idx = status->packet_idx;
|
---|
| 405 | r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
|
---|
| 406 | r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
|
---|
| 407 | r_mavlink_status->packet_rx_drop_count = status->parse_error;
|
---|
| 408 | status->parse_error = 0;
|
---|
| 409 |
|
---|
| 410 | if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
|
---|
| 411 | /*
|
---|
| 412 | the CRC came out wrong. We now need to overwrite the
|
---|
| 413 | msg CRC with the one on the wire so that if the
|
---|
| 414 | caller decides to forward the message anyway that
|
---|
| 415 | mavlink_msg_to_send_buffer() won't overwrite the
|
---|
| 416 | checksum
|
---|
| 417 | */
|
---|
| 418 | r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
|
---|
| 419 | }
|
---|
| 420 |
|
---|
| 421 | return status->msg_received;
|
---|
| 422 | }
|
---|
| 423 |
|
---|
| 424 | /**
|
---|
| 425 | * This is a convenience function which handles the complete MAVLink parsing.
|
---|
| 426 | * the function will parse one byte at a time and return the complete packet once
|
---|
| 427 | * it could be successfully decoded. This function will return 0, 1 or
|
---|
| 428 | * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
|
---|
| 429 | *
|
---|
| 430 | * Messages are parsed into an internal buffer (one for each channel). When a complete
|
---|
| 431 | * message is received it is copies into *returnMsg and the channel's status is
|
---|
| 432 | * copied into *returnStats.
|
---|
| 433 | *
|
---|
| 434 | * @param chan ID of the current channel. This allows to parse different channels with this function.
|
---|
| 435 | * a channel is not a physical message channel like a serial port, but a logic partition of
|
---|
| 436 | * the communication streams in this case. COMM_NB is the limit for the number of channels
|
---|
| 437 | * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
|
---|
| 438 | * @param c The char to parse
|
---|
| 439 | *
|
---|
| 440 | * @param returnMsg NULL if no message could be decoded, the message data else
|
---|
| 441 | * @param returnStats if a message was decoded, this is filled with the channel's stats
|
---|
| 442 | * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
|
---|
| 443 | *
|
---|
| 444 | * A typical use scenario of this function call is:
|
---|
| 445 | *
|
---|
| 446 | * @code
|
---|
| 447 | * #include <mavlink.h>
|
---|
| 448 | *
|
---|
| 449 | * mavlink_message_t msg;
|
---|
| 450 | * int chan = 0;
|
---|
| 451 | *
|
---|
| 452 | *
|
---|
| 453 | * while(serial.bytesAvailable > 0)
|
---|
| 454 | * {
|
---|
| 455 | * uint8_t byte = serial.getNextByte();
|
---|
| 456 | * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
|
---|
| 457 | * {
|
---|
| 458 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
|
---|
| 459 | * }
|
---|
| 460 | * }
|
---|
| 461 | *
|
---|
| 462 | *
|
---|
| 463 | * @endcode
|
---|
| 464 | */
|
---|
| 465 | MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
---|
| 466 | {
|
---|
| 467 | return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
|
---|
| 468 | mavlink_get_channel_status(chan),
|
---|
| 469 | c,
|
---|
| 470 | r_message,
|
---|
| 471 | r_mavlink_status);
|
---|
| 472 | }
|
---|
| 473 |
|
---|
| 474 |
|
---|
| 475 | /**
|
---|
| 476 | * This is a convenience function which handles the complete MAVLink parsing.
|
---|
| 477 | * the function will parse one byte at a time and return the complete packet once
|
---|
| 478 | * it could be successfully decoded. This function will return 0 or 1.
|
---|
| 479 | *
|
---|
| 480 | * Messages are parsed into an internal buffer (one for each channel). When a complete
|
---|
| 481 | * message is received it is copies into *returnMsg and the channel's status is
|
---|
| 482 | * copied into *returnStats.
|
---|
| 483 | *
|
---|
| 484 | * @param chan ID of the current channel. This allows to parse different channels with this function.
|
---|
| 485 | * a channel is not a physical message channel like a serial port, but a logic partition of
|
---|
| 486 | * the communication streams in this case. COMM_NB is the limit for the number of channels
|
---|
| 487 | * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
|
---|
| 488 | * @param c The char to parse
|
---|
| 489 | *
|
---|
| 490 | * @param returnMsg NULL if no message could be decoded, the message data else
|
---|
| 491 | * @param returnStats if a message was decoded, this is filled with the channel's stats
|
---|
| 492 | * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
|
---|
| 493 | *
|
---|
| 494 | * A typical use scenario of this function call is:
|
---|
| 495 | *
|
---|
| 496 | * @code
|
---|
| 497 | * #include <mavlink.h>
|
---|
| 498 | *
|
---|
| 499 | * mavlink_message_t msg;
|
---|
| 500 | * int chan = 0;
|
---|
| 501 | *
|
---|
| 502 | *
|
---|
| 503 | * while(serial.bytesAvailable > 0)
|
---|
| 504 | * {
|
---|
| 505 | * uint8_t byte = serial.getNextByte();
|
---|
| 506 | * if (mavlink_parse_char(chan, byte, &msg))
|
---|
| 507 | * {
|
---|
| 508 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
|
---|
| 509 | * }
|
---|
| 510 | * }
|
---|
| 511 | *
|
---|
| 512 | *
|
---|
| 513 | * @endcode
|
---|
| 514 | */
|
---|
| 515 | MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
---|
| 516 | {
|
---|
| 517 | uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
|
---|
| 518 | if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
|
---|
| 519 | // we got a bad CRC. Treat as a parse failure
|
---|
| 520 | mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
|
---|
| 521 | mavlink_status_t* status = mavlink_get_channel_status(chan);
|
---|
| 522 | status->parse_error++;
|
---|
| 523 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
---|
| 524 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 525 | if (c == MAVLINK_STX)
|
---|
| 526 | {
|
---|
| 527 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
---|
| 528 | rxmsg->len = 0;
|
---|
| 529 | mavlink_start_checksum(rxmsg);
|
---|
| 530 | }
|
---|
| 531 | return 0;
|
---|
| 532 | }
|
---|
| 533 | return msg_received;
|
---|
| 534 | }
|
---|
| 535 |
|
---|
| 536 | /**
|
---|
| 537 | * @brief Put a bitfield of length 1-32 bit into the buffer
|
---|
| 538 | *
|
---|
| 539 | * @param b the value to add, will be encoded in the bitfield
|
---|
| 540 | * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
|
---|
| 541 | * @param packet_index the position in the packet (the index of the first byte to use)
|
---|
| 542 | * @param bit_index the position in the byte (the index of the first bit to use)
|
---|
| 543 | * @param buffer packet buffer to write into
|
---|
| 544 | * @return new position of the last used byte in the buffer
|
---|
| 545 | */
|
---|
| 546 | MAVLINK_HELPER 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)
|
---|
| 547 | {
|
---|
| 548 | uint16_t bits_remain = bits;
|
---|
| 549 | // Transform number into network order
|
---|
| 550 | int32_t v;
|
---|
| 551 | uint8_t i_bit_index, i_byte_index, curr_bits_n;
|
---|
| 552 | #if MAVLINK_NEED_BYTE_SWAP
|
---|
| 553 | union {
|
---|
| 554 | int32_t i;
|
---|
| 555 | uint8_t b[4];
|
---|
| 556 | } bin, bout;
|
---|
| 557 | bin.i = b;
|
---|
| 558 | bout.b[0] = bin.b[3];
|
---|
| 559 | bout.b[1] = bin.b[2];
|
---|
| 560 | bout.b[2] = bin.b[1];
|
---|
| 561 | bout.b[3] = bin.b[0];
|
---|
| 562 | v = bout.i;
|
---|
| 563 | #else
|
---|
| 564 | v = b;
|
---|
| 565 | #endif
|
---|
| 566 |
|
---|
| 567 | // buffer in
|
---|
| 568 | // 01100000 01000000 00000000 11110001
|
---|
| 569 | // buffer out
|
---|
| 570 | // 11110001 00000000 01000000 01100000
|
---|
| 571 |
|
---|
| 572 | // Existing partly filled byte (four free slots)
|
---|
| 573 | // 0111xxxx
|
---|
| 574 |
|
---|
| 575 | // Mask n free bits
|
---|
| 576 | // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
|
---|
| 577 | // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
|
---|
| 578 |
|
---|
| 579 | // Shift n bits into the right position
|
---|
| 580 | // out = in >> n;
|
---|
| 581 |
|
---|
| 582 | // Mask and shift bytes
|
---|
| 583 | i_bit_index = bit_index;
|
---|
| 584 | i_byte_index = packet_index;
|
---|
| 585 | if (bit_index > 0)
|
---|
| 586 | {
|
---|
| 587 | // If bits were available at start, they were available
|
---|
| 588 | // in the byte before the current index
|
---|
| 589 | i_byte_index--;
|
---|
| 590 | }
|
---|
| 591 |
|
---|
| 592 | // While bits have not been packed yet
|
---|
| 593 | while (bits_remain > 0)
|
---|
| 594 | {
|
---|
| 595 | // Bits still have to be packed
|
---|
| 596 | // there can be more than 8 bits, so
|
---|
| 597 | // we might have to pack them into more than one byte
|
---|
| 598 |
|
---|
| 599 | // First pack everything we can into the current 'open' byte
|
---|
| 600 | //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
|
---|
| 601 | //FIXME
|
---|
| 602 | if (bits_remain <= (uint8_t)(8 - i_bit_index))
|
---|
| 603 | {
|
---|
| 604 | // Enough space
|
---|
| 605 | curr_bits_n = (uint8_t)bits_remain;
|
---|
| 606 | }
|
---|
| 607 | else
|
---|
| 608 | {
|
---|
| 609 | curr_bits_n = (8 - i_bit_index);
|
---|
| 610 | }
|
---|
| 611 |
|
---|
| 612 | // Pack these n bits into the current byte
|
---|
| 613 | // Mask out whatever was at that position with ones (xxx11111)
|
---|
| 614 | buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
|
---|
| 615 | // Put content to this position, by masking out the non-used part
|
---|
| 616 | buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
|
---|
| 617 |
|
---|
| 618 | // Increment the bit index
|
---|
| 619 | i_bit_index += curr_bits_n;
|
---|
| 620 |
|
---|
| 621 | // Now proceed to the next byte, if necessary
|
---|
| 622 | bits_remain -= curr_bits_n;
|
---|
| 623 | if (bits_remain > 0)
|
---|
| 624 | {
|
---|
| 625 | // Offer another 8 bits / one byte
|
---|
| 626 | i_byte_index++;
|
---|
| 627 | i_bit_index = 0;
|
---|
| 628 | }
|
---|
| 629 | }
|
---|
| 630 |
|
---|
| 631 | *r_bit_index = i_bit_index;
|
---|
| 632 | // If a partly filled byte is present, mark this as consumed
|
---|
| 633 | if (i_bit_index != 7) i_byte_index++;
|
---|
| 634 | return i_byte_index - packet_index;
|
---|
| 635 | }
|
---|
| 636 |
|
---|
| 637 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 638 |
|
---|
| 639 | // To make MAVLink work on your MCU, define comm_send_ch() if you wish
|
---|
| 640 | // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
|
---|
| 641 | // whole packet at a time
|
---|
| 642 |
|
---|
| 643 | /*
|
---|
| 644 |
|
---|
| 645 | #include "mavlink_types.h"
|
---|
| 646 |
|
---|
| 647 | void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
|
---|
| 648 | {
|
---|
| 649 | if (chan == MAVLINK_COMM_0)
|
---|
| 650 | {
|
---|
| 651 | uart0_transmit(ch);
|
---|
| 652 | }
|
---|
| 653 | if (chan == MAVLINK_COMM_1)
|
---|
| 654 | {
|
---|
| 655 | uart1_transmit(ch);
|
---|
| 656 | }
|
---|
| 657 | }
|
---|
| 658 | */
|
---|
| 659 |
|
---|
| 660 | MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
|
---|
| 661 | {
|
---|
| 662 | #ifdef MAVLINK_SEND_UART_BYTES
|
---|
| 663 | /* this is the more efficient approach, if the platform
|
---|
| 664 | defines it */
|
---|
| 665 | MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
|
---|
| 666 | #else
|
---|
| 667 | /* fallback to one byte at a time */
|
---|
| 668 | uint16_t i;
|
---|
| 669 | for (i = 0; i < len; i++) {
|
---|
| 670 | comm_send_ch(chan, (uint8_t)buf[i]);
|
---|
| 671 | }
|
---|
| 672 | #endif
|
---|
| 673 | }
|
---|
| 674 | #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 675 |
|
---|
| 676 | #endif /* _MAVLINK_HELPERS_H_ */
|
---|