| 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 | #include <stdio.h>
|
---|
| 9 |
|
---|
| 10 | #ifndef MAVLINK_HELPER
|
---|
| 11 | #define MAVLINK_HELPER
|
---|
| 12 | #endif
|
---|
| 13 |
|
---|
| 14 | #include "mavlink_sha256.h"
|
---|
| 15 |
|
---|
| 16 | /*
|
---|
| 17 | * Internal function to give access to the channel status for each channel
|
---|
| 18 | */
|
---|
| 19 | #ifndef MAVLINK_GET_CHANNEL_STATUS
|
---|
| 20 | MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
|
---|
| 21 | {
|
---|
| 22 | #ifdef MAVLINK_EXTERNAL_RX_STATUS
|
---|
| 23 | // No m_mavlink_status array defined in function,
|
---|
| 24 | // has to be defined externally
|
---|
| 25 | #else
|
---|
| 26 | static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
---|
| 27 | #endif
|
---|
| 28 | return &m_mavlink_status[chan];
|
---|
| 29 | }
|
---|
| 30 | #endif
|
---|
| 31 |
|
---|
| 32 | /*
|
---|
| 33 | * Internal function to give access to the channel buffer for each channel
|
---|
| 34 | */
|
---|
| 35 | #ifndef MAVLINK_GET_CHANNEL_BUFFER
|
---|
| 36 | MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
|
---|
| 37 | {
|
---|
| 38 |
|
---|
| 39 | #ifdef MAVLINK_EXTERNAL_RX_BUFFER
|
---|
| 40 | // No m_mavlink_buffer array defined in function,
|
---|
| 41 | // has to be defined externally
|
---|
| 42 | #else
|
---|
| 43 | static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
|
---|
| 44 | #endif
|
---|
| 45 | return &m_mavlink_buffer[chan];
|
---|
| 46 | }
|
---|
| 47 | #endif // MAVLINK_GET_CHANNEL_BUFFER
|
---|
| 48 |
|
---|
| 49 | /**
|
---|
| 50 | * @brief Reset the status of a channel.
|
---|
| 51 | */
|
---|
| 52 | MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
|
---|
| 53 | {
|
---|
| 54 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 55 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 56 | }
|
---|
| 57 |
|
---|
| 58 | /**
|
---|
| 59 | * @brief create a signature block for a packet
|
---|
| 60 | */
|
---|
| 61 | MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
|
---|
| 62 | uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
|
---|
| 63 | const uint8_t *header, uint8_t header_len,
|
---|
| 64 | const uint8_t *packet, uint8_t packet_len,
|
---|
| 65 | const uint8_t crc[2])
|
---|
| 66 | {
|
---|
| 67 | mavlink_sha256_ctx ctx;
|
---|
| 68 | union {
|
---|
| 69 | uint64_t t64;
|
---|
| 70 | uint8_t t8[8];
|
---|
| 71 | } tstamp;
|
---|
| 72 | if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
|
---|
| 73 | return 0;
|
---|
| 74 | }
|
---|
| 75 | signature[0] = signing->link_id;
|
---|
| 76 | tstamp.t64 = signing->timestamp;
|
---|
| 77 | memcpy(&signature[1], tstamp.t8, 6);
|
---|
| 78 | signing->timestamp++;
|
---|
| 79 |
|
---|
| 80 | mavlink_sha256_init(&ctx);
|
---|
| 81 | mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
|
---|
| 82 | mavlink_sha256_update(&ctx, header, header_len);
|
---|
| 83 | mavlink_sha256_update(&ctx, packet, packet_len);
|
---|
| 84 | mavlink_sha256_update(&ctx, crc, 2);
|
---|
| 85 | mavlink_sha256_update(&ctx, signature, 7);
|
---|
| 86 | mavlink_sha256_final_48(&ctx, &signature[7]);
|
---|
| 87 |
|
---|
| 88 | return MAVLINK_SIGNATURE_BLOCK_LEN;
|
---|
| 89 | }
|
---|
| 90 |
|
---|
| 91 | /**
|
---|
| 92 | * return new packet length for trimming payload of any trailing zero
|
---|
| 93 | * bytes. Used in MAVLink2 to give simple support for variable length
|
---|
| 94 | * arrays.
|
---|
| 95 | */
|
---|
| 96 | MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length)
|
---|
| 97 | {
|
---|
| 98 | while (length > 0 && payload[length-1] == 0) {
|
---|
| 99 | length--;
|
---|
| 100 | }
|
---|
| 101 | return length;
|
---|
| 102 | }
|
---|
| 103 |
|
---|
| 104 | /**
|
---|
| 105 | * @brief check a signature block for a packet
|
---|
| 106 | */
|
---|
| 107 | MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing,
|
---|
| 108 | mavlink_signing_streams_t *signing_streams,
|
---|
| 109 | const mavlink_message_t *msg)
|
---|
| 110 | {
|
---|
| 111 | if (signing == NULL) {
|
---|
| 112 | return true;
|
---|
| 113 | }
|
---|
| 114 | const uint8_t *p = (const uint8_t *)&msg->magic;
|
---|
| 115 | const uint8_t *psig = msg->signature;
|
---|
| 116 | const uint8_t *incoming_signature = psig+7;
|
---|
| 117 | mavlink_sha256_ctx ctx;
|
---|
| 118 | uint8_t signature[6];
|
---|
| 119 | uint16_t i;
|
---|
| 120 |
|
---|
| 121 | mavlink_sha256_init(&ctx);
|
---|
| 122 | mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
|
---|
| 123 | mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len);
|
---|
| 124 | mavlink_sha256_update(&ctx, msg->ck, 2);
|
---|
| 125 | mavlink_sha256_update(&ctx, psig, 1+6);
|
---|
| 126 | mavlink_sha256_final_48(&ctx, signature);
|
---|
| 127 | if (memcmp(signature, incoming_signature, 6) != 0) {
|
---|
| 128 | return false;
|
---|
| 129 | }
|
---|
| 130 |
|
---|
| 131 | // now check timestamp
|
---|
| 132 | union tstamp {
|
---|
| 133 | uint64_t t64;
|
---|
| 134 | uint8_t t8[8];
|
---|
| 135 | } tstamp;
|
---|
| 136 | uint8_t link_id = psig[0];
|
---|
| 137 | tstamp.t64 = 0;
|
---|
| 138 | memcpy(tstamp.t8, psig+1, 6);
|
---|
| 139 |
|
---|
| 140 | if (signing_streams == NULL) {
|
---|
| 141 | return false;
|
---|
| 142 | }
|
---|
| 143 |
|
---|
| 144 | // find stream
|
---|
| 145 | for (i=0; i<signing_streams->num_signing_streams; i++) {
|
---|
| 146 | if (msg->sysid == signing_streams->stream[i].sysid &&
|
---|
| 147 | msg->compid == signing_streams->stream[i].compid &&
|
---|
| 148 | link_id == signing_streams->stream[i].link_id) {
|
---|
| 149 | break;
|
---|
| 150 | }
|
---|
| 151 | }
|
---|
| 152 | if (i == signing_streams->num_signing_streams) {
|
---|
| 153 | if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) {
|
---|
| 154 | // over max number of streams
|
---|
| 155 | return false;
|
---|
| 156 | }
|
---|
| 157 | // new stream. Only accept if timestamp is not more than 1 minute old
|
---|
| 158 | if (tstamp.t64 + 6000*1000UL < signing->timestamp) {
|
---|
| 159 | return false;
|
---|
| 160 | }
|
---|
| 161 | // add new stream
|
---|
| 162 | signing_streams->stream[i].sysid = msg->sysid;
|
---|
| 163 | signing_streams->stream[i].compid = msg->compid;
|
---|
| 164 | signing_streams->stream[i].link_id = link_id;
|
---|
| 165 | signing_streams->num_signing_streams++;
|
---|
| 166 | } else {
|
---|
| 167 | union tstamp last_tstamp;
|
---|
| 168 | last_tstamp.t64 = 0;
|
---|
| 169 | memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6);
|
---|
| 170 | if (tstamp.t64 <= last_tstamp.t64) {
|
---|
| 171 | // repeating old timestamp
|
---|
| 172 | return false;
|
---|
| 173 | }
|
---|
| 174 | }
|
---|
| 175 |
|
---|
| 176 | // remember last timestamp
|
---|
| 177 | memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6);
|
---|
| 178 |
|
---|
| 179 | // our next timestamp must be at least this timestamp
|
---|
| 180 | if (tstamp.t64 > signing->timestamp) {
|
---|
| 181 | signing->timestamp = tstamp.t64;
|
---|
| 182 | }
|
---|
| 183 | return true;
|
---|
| 184 | }
|
---|
| 185 |
|
---|
| 186 |
|
---|
| 187 | /**
|
---|
| 188 | * @brief Finalize a MAVLink message with channel assignment
|
---|
| 189 | *
|
---|
| 190 | * This function calculates the checksum and sets length and aircraft id correctly.
|
---|
| 191 | * It assumes that the message id and the payload are already correctly set. This function
|
---|
| 192 | * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
|
---|
| 193 | * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
|
---|
| 194 | *
|
---|
| 195 | * @param msg Message to finalize
|
---|
| 196 | * @param system_id Id of the sending (this) system, 1-127
|
---|
| 197 | * @param length Message length
|
---|
| 198 | */
|
---|
| 199 | MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
---|
| 200 | uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
---|
| 201 | {
|
---|
| 202 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 203 | bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
|
---|
| 204 | bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
|
---|
| 205 | uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
|
---|
| 206 | uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
|
---|
| 207 | uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
|
---|
| 208 | if (mavlink1) {
|
---|
| 209 | msg->magic = MAVLINK_STX_MAVLINK1;
|
---|
| 210 | header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
|
---|
| 211 | } else {
|
---|
| 212 | msg->magic = MAVLINK_STX;
|
---|
| 213 | }
|
---|
| 214 | msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
|
---|
| 215 | msg->sysid = system_id;
|
---|
| 216 | msg->compid = component_id;
|
---|
| 217 | msg->incompat_flags = 0;
|
---|
| 218 | if (signing) {
|
---|
| 219 | msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
|
---|
| 220 | }
|
---|
| 221 | msg->compat_flags = 0;
|
---|
| 222 | msg->seq = status->current_tx_seq;
|
---|
| 223 | status->current_tx_seq = status->current_tx_seq + 1;
|
---|
| 224 |
|
---|
| 225 | // form the header as a byte array for the crc
|
---|
| 226 | buf[0] = msg->magic;
|
---|
| 227 | buf[1] = msg->len;
|
---|
| 228 | if (mavlink1) {
|
---|
| 229 | buf[2] = msg->seq;
|
---|
| 230 | buf[3] = msg->sysid;
|
---|
| 231 | buf[4] = msg->compid;
|
---|
| 232 | buf[5] = msg->msgid & 0xFF;
|
---|
| 233 | } else {
|
---|
| 234 | buf[2] = msg->incompat_flags;
|
---|
| 235 | buf[3] = msg->compat_flags;
|
---|
| 236 | buf[4] = msg->seq;
|
---|
| 237 | buf[5] = msg->sysid;
|
---|
| 238 | buf[6] = msg->compid;
|
---|
| 239 | buf[7] = msg->msgid & 0xFF;
|
---|
| 240 | buf[8] = (msg->msgid >> 8) & 0xFF;
|
---|
| 241 | buf[9] = (msg->msgid >> 16) & 0xFF;
|
---|
| 242 | }
|
---|
| 243 |
|
---|
| 244 | msg->checksum = crc_calculate(&buf[1], header_len-1);
|
---|
| 245 | crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
|
---|
| 246 | crc_accumulate(crc_extra, &msg->checksum);
|
---|
| 247 | mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
|
---|
| 248 | mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
|
---|
| 249 |
|
---|
| 250 | if (signing) {
|
---|
| 251 | mavlink_sign_packet(status->signing,
|
---|
| 252 | msg->signature,
|
---|
| 253 | (const uint8_t *)buf, header_len,
|
---|
| 254 | (const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
|
---|
| 255 | (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
|
---|
| 256 | }
|
---|
| 257 |
|
---|
| 258 | return msg->len + header_len + 2 + signature_len;
|
---|
| 259 | }
|
---|
| 260 |
|
---|
| 261 |
|
---|
| 262 | /**
|
---|
| 263 | * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
|
---|
| 264 | */
|
---|
| 265 | MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
---|
| 266 | uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
---|
| 267 | {
|
---|
| 268 | return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
|
---|
| 269 | }
|
---|
| 270 |
|
---|
| 271 | static inline void _mav_parse_error(mavlink_status_t *status)
|
---|
| 272 | {
|
---|
| 273 | status->parse_error++;
|
---|
| 274 | }
|
---|
| 275 |
|
---|
| 276 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 277 | MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
|
---|
| 278 |
|
---|
| 279 | /**
|
---|
| 280 | * @brief Finalize a MAVLink message with channel assignment and send
|
---|
| 281 | */
|
---|
| 282 | MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,
|
---|
| 283 | const char *packet,
|
---|
| 284 | uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
---|
| 285 | {
|
---|
| 286 | uint16_t checksum;
|
---|
| 287 | uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
|
---|
| 288 | uint8_t ck[2];
|
---|
| 289 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 290 | uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
|
---|
| 291 | uint8_t signature_len = 0;
|
---|
| 292 | uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
|
---|
| 293 | bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
|
---|
| 294 | bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
|
---|
| 295 |
|
---|
| 296 | if (mavlink1) {
|
---|
| 297 | length = min_length;
|
---|
| 298 | if (msgid > 255) {
|
---|
| 299 | // can't send 16 bit messages
|
---|
| 300 | _mav_parse_error(status);
|
---|
| 301 | return;
|
---|
| 302 | }
|
---|
| 303 | header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
|
---|
| 304 | buf[0] = MAVLINK_STX_MAVLINK1;
|
---|
| 305 | buf[1] = length;
|
---|
| 306 | buf[2] = status->current_tx_seq;
|
---|
| 307 | buf[3] = mavlink_system.sysid;
|
---|
| 308 | buf[4] = mavlink_system.compid;
|
---|
| 309 | buf[5] = msgid & 0xFF;
|
---|
| 310 | } else {
|
---|
| 311 | uint8_t incompat_flags = 0;
|
---|
| 312 | if (signing) {
|
---|
| 313 | incompat_flags |= MAVLINK_IFLAG_SIGNED;
|
---|
| 314 | }
|
---|
| 315 | length = _mav_trim_payload(packet, length);
|
---|
| 316 | buf[0] = MAVLINK_STX;
|
---|
| 317 | buf[1] = length;
|
---|
| 318 | buf[2] = incompat_flags;
|
---|
| 319 | buf[3] = 0; // compat_flags
|
---|
| 320 | buf[4] = status->current_tx_seq;
|
---|
| 321 | buf[5] = mavlink_system.sysid;
|
---|
| 322 | buf[6] = mavlink_system.compid;
|
---|
| 323 | buf[7] = msgid & 0xFF;
|
---|
| 324 | buf[8] = (msgid >> 8) & 0xFF;
|
---|
| 325 | buf[9] = (msgid >> 16) & 0xFF;
|
---|
| 326 | }
|
---|
| 327 | status->current_tx_seq++;
|
---|
| 328 | checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
|
---|
| 329 | crc_accumulate_buffer(&checksum, packet, length);
|
---|
| 330 | crc_accumulate(crc_extra, &checksum);
|
---|
| 331 | ck[0] = (uint8_t)(checksum & 0xFF);
|
---|
| 332 | ck[1] = (uint8_t)(checksum >> 8);
|
---|
| 333 |
|
---|
| 334 | if (signing) {
|
---|
| 335 | // possibly add a signature
|
---|
| 336 | signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,
|
---|
| 337 | (const uint8_t *)packet, length, ck);
|
---|
| 338 | }
|
---|
| 339 |
|
---|
| 340 | MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
|
---|
| 341 | _mavlink_send_uart(chan, (const char *)buf, header_len+1);
|
---|
| 342 | _mavlink_send_uart(chan, packet, length);
|
---|
| 343 | _mavlink_send_uart(chan, (const char *)ck, 2);
|
---|
| 344 | if (signature_len != 0) {
|
---|
| 345 | _mavlink_send_uart(chan, (const char *)signature, signature_len);
|
---|
| 346 | }
|
---|
| 347 | MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
|
---|
| 348 | }
|
---|
| 349 |
|
---|
| 350 | /**
|
---|
| 351 | * @brief re-send a message over a uart channel
|
---|
| 352 | * this is more stack efficient than re-marshalling the message
|
---|
| 353 | * If the message is signed then the original signature is also sent
|
---|
| 354 | */
|
---|
| 355 | MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
|
---|
| 356 | {
|
---|
| 357 | uint8_t ck[2];
|
---|
| 358 |
|
---|
| 359 | ck[0] = (uint8_t)(msg->checksum & 0xFF);
|
---|
| 360 | ck[1] = (uint8_t)(msg->checksum >> 8);
|
---|
| 361 | // XXX use the right sequence here
|
---|
| 362 |
|
---|
| 363 | uint8_t header_len;
|
---|
| 364 | uint8_t signature_len;
|
---|
| 365 |
|
---|
| 366 | if (msg->magic == MAVLINK_STX_MAVLINK1) {
|
---|
| 367 | header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
|
---|
| 368 | signature_len = 0;
|
---|
| 369 | MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
|
---|
| 370 | // we can't send the structure directly as it has extra mavlink2 elements in it
|
---|
| 371 | uint8_t buf[header_len];
|
---|
| 372 | buf[0] = msg->magic;
|
---|
| 373 | buf[1] = msg->len;
|
---|
| 374 | buf[2] = msg->seq;
|
---|
| 375 | buf[3] = msg->sysid;
|
---|
| 376 | buf[4] = msg->compid;
|
---|
| 377 | buf[5] = msg->msgid & 0xFF;
|
---|
| 378 | _mavlink_send_uart(chan, (const char*)buf, header_len);
|
---|
| 379 | } else {
|
---|
| 380 | header_len = MAVLINK_CORE_HEADER_LEN + 1;
|
---|
| 381 | signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
|
---|
| 382 | MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
|
---|
| 383 | uint8_t buf[header_len];
|
---|
| 384 | buf[0] = msg->magic;
|
---|
| 385 | buf[1] = msg->len;
|
---|
| 386 | buf[2] = msg->incompat_flags;
|
---|
| 387 | buf[3] = msg->compat_flags;
|
---|
| 388 | buf[4] = msg->seq;
|
---|
| 389 | buf[5] = msg->sysid;
|
---|
| 390 | buf[6] = msg->compid;
|
---|
| 391 | buf[7] = msg->msgid & 0xFF;
|
---|
| 392 | buf[8] = (msg->msgid >> 8) & 0xFF;
|
---|
| 393 | buf[9] = (msg->msgid >> 16) & 0xFF;
|
---|
| 394 | _mavlink_send_uart(chan, (const char *)buf, header_len);
|
---|
| 395 | }
|
---|
| 396 | _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
|
---|
| 397 | _mavlink_send_uart(chan, (const char *)ck, 2);
|
---|
| 398 | if (signature_len != 0) {
|
---|
| 399 | _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN);
|
---|
| 400 | }
|
---|
| 401 | MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
|
---|
| 402 | }
|
---|
| 403 | #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 404 |
|
---|
| 405 | /**
|
---|
| 406 | * @brief Pack a message to send it over a serial byte stream
|
---|
| 407 | */
|
---|
| 408 | MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
|
---|
| 409 | {
|
---|
| 410 | uint8_t signature_len, header_len;
|
---|
| 411 | uint8_t *ck;
|
---|
| 412 | uint8_t length = msg->len;
|
---|
| 413 |
|
---|
| 414 | if (msg->magic == MAVLINK_STX_MAVLINK1) {
|
---|
| 415 | signature_len = 0;
|
---|
| 416 | header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
|
---|
| 417 | buf[0] = msg->magic;
|
---|
| 418 | buf[1] = length;
|
---|
| 419 | buf[2] = msg->seq;
|
---|
| 420 | buf[3] = msg->sysid;
|
---|
| 421 | buf[4] = msg->compid;
|
---|
| 422 | buf[5] = msg->msgid & 0xFF;
|
---|
| 423 | memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
|
---|
| 424 | ck = buf + header_len + 1 + (uint16_t)msg->len;
|
---|
| 425 | } else {
|
---|
| 426 | length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
|
---|
| 427 | header_len = MAVLINK_CORE_HEADER_LEN;
|
---|
| 428 | buf[0] = msg->magic;
|
---|
| 429 | buf[1] = length;
|
---|
| 430 | buf[2] = msg->incompat_flags;
|
---|
| 431 | buf[3] = msg->compat_flags;
|
---|
| 432 | buf[4] = msg->seq;
|
---|
| 433 | buf[5] = msg->sysid;
|
---|
| 434 | buf[6] = msg->compid;
|
---|
| 435 | buf[7] = msg->msgid & 0xFF;
|
---|
| 436 | buf[8] = (msg->msgid >> 8) & 0xFF;
|
---|
| 437 | buf[9] = (msg->msgid >> 16) & 0xFF;
|
---|
| 438 | memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
|
---|
| 439 | ck = buf + header_len + 1 + (uint16_t)length;
|
---|
| 440 | signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
|
---|
| 441 | }
|
---|
| 442 | ck[0] = (uint8_t)(msg->checksum & 0xFF);
|
---|
| 443 | ck[1] = (uint8_t)(msg->checksum >> 8);
|
---|
| 444 | if (signature_len > 0) {
|
---|
| 445 | memcpy(&ck[2], msg->signature, signature_len);
|
---|
| 446 | }
|
---|
| 447 |
|
---|
| 448 | return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
|
---|
| 449 | }
|
---|
| 450 |
|
---|
| 451 | union __mavlink_bitfield {
|
---|
| 452 | uint8_t uint8;
|
---|
| 453 | int8_t int8;
|
---|
| 454 | uint16_t uint16;
|
---|
| 455 | int16_t int16;
|
---|
| 456 | uint32_t uint32;
|
---|
| 457 | int32_t int32;
|
---|
| 458 | };
|
---|
| 459 |
|
---|
| 460 |
|
---|
| 461 | MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
|
---|
| 462 | {
|
---|
| 463 | crc_init(&msg->checksum);
|
---|
| 464 | }
|
---|
| 465 |
|
---|
| 466 | MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
|
---|
| 467 | {
|
---|
| 468 | crc_accumulate(c, &msg->checksum);
|
---|
| 469 | }
|
---|
| 470 |
|
---|
| 471 | /*
|
---|
| 472 | return the crc_entry value for a msgid
|
---|
| 473 | */
|
---|
| 474 | MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
|
---|
| 475 | {
|
---|
| 476 | static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
|
---|
| 477 | /*
|
---|
| 478 | use a bisection search to find the right entry. A perfect hash may be better
|
---|
| 479 | Note that this assumes the table is sorted by msgid
|
---|
| 480 | */
|
---|
| 481 | uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]);
|
---|
| 482 | while (low < high) {
|
---|
| 483 | uint32_t mid = (low+1+high)/2;
|
---|
| 484 | if (msgid < mavlink_message_crcs[mid].msgid) {
|
---|
| 485 | high = mid-1;
|
---|
| 486 | continue;
|
---|
| 487 | }
|
---|
| 488 | if (msgid > mavlink_message_crcs[mid].msgid) {
|
---|
| 489 | low = mid;
|
---|
| 490 | continue;
|
---|
| 491 | }
|
---|
| 492 | low = mid;
|
---|
| 493 | break;
|
---|
| 494 | }
|
---|
| 495 | if (mavlink_message_crcs[low].msgid != msgid) {
|
---|
| 496 | // msgid is not in the table
|
---|
| 497 | return NULL;
|
---|
| 498 | }
|
---|
| 499 | return &mavlink_message_crcs[low];
|
---|
| 500 | }
|
---|
| 501 |
|
---|
| 502 | /*
|
---|
| 503 | return the crc_extra value for a message
|
---|
| 504 | */
|
---|
| 505 | MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg)
|
---|
| 506 | {
|
---|
| 507 | const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
|
---|
| 508 | return e?e->crc_extra:0;
|
---|
| 509 | }
|
---|
| 510 |
|
---|
| 511 | /*
|
---|
| 512 | return the expected message length
|
---|
| 513 | */
|
---|
| 514 | #define MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH
|
---|
| 515 | MAVLINK_HELPER uint8_t mavlink_expected_message_length(const mavlink_message_t *msg)
|
---|
| 516 | {
|
---|
| 517 | const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
|
---|
| 518 | return e?e->msg_len:0;
|
---|
| 519 | }
|
---|
| 520 |
|
---|
| 521 | /**
|
---|
| 522 | * This is a varient of mavlink_frame_char() but with caller supplied
|
---|
| 523 | * parsing buffers. It is useful when you want to create a MAVLink
|
---|
| 524 | * parser in a library that doesn't use any global variables
|
---|
| 525 | *
|
---|
| 526 | * @param rxmsg parsing message buffer
|
---|
| 527 | * @param status parsing starus buffer
|
---|
| 528 | * @param c The char to parse
|
---|
| 529 | *
|
---|
| 530 | * @param returnMsg NULL if no message could be decoded, the message data else
|
---|
| 531 | * @param returnStats if a message was decoded, this is filled with the channel's stats
|
---|
| 532 | * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
|
---|
| 533 | *
|
---|
| 534 | * A typical use scenario of this function call is:
|
---|
| 535 | *
|
---|
| 536 | * @code
|
---|
| 537 | * #include <mavlink.h>
|
---|
| 538 | *
|
---|
| 539 | * mavlink_message_t msg;
|
---|
| 540 | * int chan = 0;
|
---|
| 541 | *
|
---|
| 542 | *
|
---|
| 543 | * while(serial.bytesAvailable > 0)
|
---|
| 544 | * {
|
---|
| 545 | * uint8_t byte = serial.getNextByte();
|
---|
| 546 | * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
|
---|
| 547 | * {
|
---|
| 548 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
|
---|
| 549 | * }
|
---|
| 550 | * }
|
---|
| 551 | *
|
---|
| 552 | *
|
---|
| 553 | * @endcode
|
---|
| 554 | */
|
---|
| 555 | MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
---|
| 556 | mavlink_status_t* status,
|
---|
| 557 | uint8_t c,
|
---|
| 558 | mavlink_message_t* r_message,
|
---|
| 559 | mavlink_status_t* r_mavlink_status)
|
---|
| 560 | {
|
---|
| 561 | /* Enable this option to check the length of each message.
|
---|
| 562 | This allows invalid messages to be caught much sooner. Use if the transmission
|
---|
| 563 | medium is prone to missing (or extra) characters (e.g. a radio that fades in
|
---|
| 564 | and out). Only use if the channel will only contain messages types listed in
|
---|
| 565 | the headers.
|
---|
| 566 | */
|
---|
| 567 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
---|
| 568 | #ifndef MAVLINK_MESSAGE_LENGTH
|
---|
| 569 | static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
---|
| 570 | #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
|
---|
| 571 | #endif
|
---|
| 572 | #endif
|
---|
| 573 |
|
---|
| 574 | int bufferIndex = 0;
|
---|
| 575 |
|
---|
| 576 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
---|
| 577 |
|
---|
| 578 | switch (status->parse_state)
|
---|
| 579 | {
|
---|
| 580 | case MAVLINK_PARSE_STATE_UNINIT:
|
---|
| 581 | case MAVLINK_PARSE_STATE_IDLE:
|
---|
| 582 | if (c == MAVLINK_STX)
|
---|
| 583 | {
|
---|
| 584 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
---|
| 585 | rxmsg->len = 0;
|
---|
| 586 | rxmsg->magic = c;
|
---|
| 587 | status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
|
---|
| 588 | mavlink_start_checksum(rxmsg);
|
---|
| 589 | } else if (c == MAVLINK_STX_MAVLINK1)
|
---|
| 590 | {
|
---|
| 591 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
---|
| 592 | rxmsg->len = 0;
|
---|
| 593 | rxmsg->magic = c;
|
---|
| 594 | status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
|
---|
| 595 | mavlink_start_checksum(rxmsg);
|
---|
| 596 | }
|
---|
| 597 | break;
|
---|
| 598 |
|
---|
| 599 | case MAVLINK_PARSE_STATE_GOT_STX:
|
---|
| 600 | if (status->msg_received
|
---|
| 601 | /* Support shorter buffers than the
|
---|
| 602 | default maximum packet size */
|
---|
| 603 | #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
|
---|
| 604 | || c > MAVLINK_MAX_PAYLOAD_LEN
|
---|
| 605 | #endif
|
---|
| 606 | )
|
---|
| 607 | {
|
---|
| 608 | status->buffer_overrun++;
|
---|
| 609 | _mav_parse_error(status);
|
---|
| 610 | status->msg_received = 0;
|
---|
| 611 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 612 | }
|
---|
| 613 | else
|
---|
| 614 | {
|
---|
| 615 | // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
|
---|
| 616 | rxmsg->len = c;
|
---|
| 617 | status->packet_idx = 0;
|
---|
| 618 | mavlink_update_checksum(rxmsg, c);
|
---|
| 619 | if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
|
---|
| 620 | rxmsg->incompat_flags = 0;
|
---|
| 621 | rxmsg->compat_flags = 0;
|
---|
| 622 | status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
|
---|
| 623 | } else {
|
---|
| 624 | status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
|
---|
| 625 | }
|
---|
| 626 | }
|
---|
| 627 | break;
|
---|
| 628 |
|
---|
| 629 | case MAVLINK_PARSE_STATE_GOT_LENGTH:
|
---|
| 630 | rxmsg->incompat_flags = c;
|
---|
| 631 | if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
|
---|
| 632 | // message includes an incompatible feature flag
|
---|
| 633 | _mav_parse_error(status);
|
---|
| 634 | status->msg_received = 0;
|
---|
| 635 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 636 | break;
|
---|
| 637 | }
|
---|
| 638 | mavlink_update_checksum(rxmsg, c);
|
---|
| 639 | status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
|
---|
| 640 | break;
|
---|
| 641 |
|
---|
| 642 | case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
|
---|
| 643 | rxmsg->compat_flags = c;
|
---|
| 644 | mavlink_update_checksum(rxmsg, c);
|
---|
| 645 | status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
|
---|
| 646 | break;
|
---|
| 647 |
|
---|
| 648 | case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
|
---|
| 649 | rxmsg->seq = c;
|
---|
| 650 | mavlink_update_checksum(rxmsg, c);
|
---|
| 651 | status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
|
---|
| 652 | break;
|
---|
| 653 |
|
---|
| 654 | case MAVLINK_PARSE_STATE_GOT_SEQ:
|
---|
| 655 | rxmsg->sysid = c;
|
---|
| 656 | mavlink_update_checksum(rxmsg, c);
|
---|
| 657 | status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
|
---|
| 658 | break;
|
---|
| 659 |
|
---|
| 660 | case MAVLINK_PARSE_STATE_GOT_SYSID:
|
---|
| 661 | rxmsg->compid = c;
|
---|
| 662 | mavlink_update_checksum(rxmsg, c);
|
---|
| 663 | status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
|
---|
| 664 | break;
|
---|
| 665 |
|
---|
| 666 | case MAVLINK_PARSE_STATE_GOT_COMPID:
|
---|
| 667 | rxmsg->msgid = c;
|
---|
| 668 | mavlink_update_checksum(rxmsg, c);
|
---|
| 669 | if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
|
---|
| 670 | status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
|
---|
| 671 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
---|
| 672 | if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
|
---|
| 673 | {
|
---|
| 674 | _mav_parse_error(status);
|
---|
| 675 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 676 | break;
|
---|
| 677 | }
|
---|
| 678 | #endif
|
---|
| 679 | } else {
|
---|
| 680 | status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
|
---|
| 681 | }
|
---|
| 682 | break;
|
---|
| 683 |
|
---|
| 684 | case MAVLINK_PARSE_STATE_GOT_MSGID1:
|
---|
| 685 | rxmsg->msgid |= c<<8;
|
---|
| 686 | mavlink_update_checksum(rxmsg, c);
|
---|
| 687 | status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
|
---|
| 688 | break;
|
---|
| 689 |
|
---|
| 690 | case MAVLINK_PARSE_STATE_GOT_MSGID2:
|
---|
| 691 | rxmsg->msgid |= c<<16;
|
---|
| 692 | mavlink_update_checksum(rxmsg, c);
|
---|
| 693 | status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
|
---|
| 694 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
---|
| 695 | if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
|
---|
| 696 | {
|
---|
| 697 | _mav_parse_error(status);
|
---|
| 698 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 699 | break;
|
---|
| 700 | }
|
---|
| 701 | #endif
|
---|
| 702 | break;
|
---|
| 703 |
|
---|
| 704 | case MAVLINK_PARSE_STATE_GOT_MSGID3:
|
---|
| 705 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
|
---|
| 706 | mavlink_update_checksum(rxmsg, c);
|
---|
| 707 | if (status->packet_idx == rxmsg->len)
|
---|
| 708 | {
|
---|
| 709 | status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
|
---|
| 710 | }
|
---|
| 711 | break;
|
---|
| 712 |
|
---|
| 713 | case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
|
---|
| 714 | const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
|
---|
| 715 | uint8_t crc_extra = e?e->crc_extra:0;
|
---|
| 716 | mavlink_update_checksum(rxmsg, crc_extra);
|
---|
| 717 | if (c != (rxmsg->checksum & 0xFF)) {
|
---|
| 718 | status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
|
---|
| 719 | } else {
|
---|
| 720 | status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
|
---|
| 721 | }
|
---|
| 722 | rxmsg->ck[0] = c;
|
---|
| 723 |
|
---|
| 724 | // zero-fill the packet to cope with short incoming packets
|
---|
| 725 | if (e && status->packet_idx < e->msg_len) {
|
---|
| 726 | memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->msg_len - status->packet_idx);
|
---|
| 727 | }
|
---|
| 728 | break;
|
---|
| 729 | }
|
---|
| 730 |
|
---|
| 731 | case MAVLINK_PARSE_STATE_GOT_CRC1:
|
---|
| 732 | case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
|
---|
| 733 | if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) {
|
---|
| 734 | // got a bad CRC message
|
---|
| 735 | status->msg_received = MAVLINK_FRAMING_BAD_CRC;
|
---|
| 736 | } else {
|
---|
| 737 | // Successfully got message
|
---|
| 738 | status->msg_received = MAVLINK_FRAMING_OK;
|
---|
| 739 | }
|
---|
| 740 | rxmsg->ck[1] = c;
|
---|
| 741 | if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
|
---|
| 742 | status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
|
---|
| 743 | status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
|
---|
| 744 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
---|
| 745 | } else {
|
---|
| 746 | if (status->signing &&
|
---|
| 747 | (status->signing->accept_unsigned_callback == NULL ||
|
---|
| 748 | !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
|
---|
| 749 | // don't accept this unsigned packet
|
---|
| 750 | status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
|
---|
| 751 | }
|
---|
| 752 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 753 | memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
|
---|
| 754 | }
|
---|
| 755 | break;
|
---|
| 756 | case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
|
---|
| 757 | rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
|
---|
| 758 | status->signature_wait--;
|
---|
| 759 | if (status->signature_wait == 0) {
|
---|
| 760 | // we have the whole signature, check it is OK
|
---|
| 761 | bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
|
---|
| 762 | if (!sig_ok &&
|
---|
| 763 | (status->signing->accept_unsigned_callback &&
|
---|
| 764 | status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
|
---|
| 765 | // accepted via application level override
|
---|
| 766 | sig_ok = true;
|
---|
| 767 | }
|
---|
| 768 | if (sig_ok) {
|
---|
| 769 | status->msg_received = MAVLINK_FRAMING_OK;
|
---|
| 770 | } else {
|
---|
| 771 | status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
|
---|
| 772 | }
|
---|
| 773 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 774 | memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
|
---|
| 775 | }
|
---|
| 776 | break;
|
---|
| 777 | }
|
---|
| 778 |
|
---|
| 779 | bufferIndex++;
|
---|
| 780 | // If a message has been sucessfully decoded, check index
|
---|
| 781 | if (status->msg_received == MAVLINK_FRAMING_OK)
|
---|
| 782 | {
|
---|
| 783 | //while(status->current_seq != rxmsg->seq)
|
---|
| 784 | //{
|
---|
| 785 | // status->packet_rx_drop_count++;
|
---|
| 786 | // status->current_seq++;
|
---|
| 787 | //}
|
---|
| 788 | status->current_rx_seq = rxmsg->seq;
|
---|
| 789 | // Initial condition: If no packet has been received so far, drop count is undefined
|
---|
| 790 | if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
|
---|
| 791 | // Count this packet as received
|
---|
| 792 | status->packet_rx_success_count++;
|
---|
| 793 | }
|
---|
| 794 |
|
---|
| 795 | r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg
|
---|
| 796 | r_mavlink_status->parse_state = status->parse_state;
|
---|
| 797 | r_mavlink_status->packet_idx = status->packet_idx;
|
---|
| 798 | r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
|
---|
| 799 | r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
|
---|
| 800 | r_mavlink_status->packet_rx_drop_count = status->parse_error;
|
---|
| 801 | r_mavlink_status->flags = status->flags;
|
---|
| 802 | status->parse_error = 0;
|
---|
| 803 |
|
---|
| 804 | if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
|
---|
| 805 | /*
|
---|
| 806 | the CRC came out wrong. We now need to overwrite the
|
---|
| 807 | msg CRC with the one on the wire so that if the
|
---|
| 808 | caller decides to forward the message anyway that
|
---|
| 809 | mavlink_msg_to_send_buffer() won't overwrite the
|
---|
| 810 | checksum
|
---|
| 811 | */
|
---|
| 812 | r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
|
---|
| 813 | }
|
---|
| 814 |
|
---|
| 815 | return status->msg_received;
|
---|
| 816 | }
|
---|
| 817 |
|
---|
| 818 | /**
|
---|
| 819 | * This is a convenience function which handles the complete MAVLink parsing.
|
---|
| 820 | * the function will parse one byte at a time and return the complete packet once
|
---|
| 821 | * it could be successfully decoded. This function will return 0, 1 or
|
---|
| 822 | * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
|
---|
| 823 | *
|
---|
| 824 | * Messages are parsed into an internal buffer (one for each channel). When a complete
|
---|
| 825 | * message is received it is copies into *returnMsg and the channel's status is
|
---|
| 826 | * copied into *returnStats.
|
---|
| 827 | *
|
---|
| 828 | * @param chan ID of the current channel. This allows to parse different channels with this function.
|
---|
| 829 | * a channel is not a physical message channel like a serial port, but a logic partition of
|
---|
| 830 | * the communication streams in this case. COMM_NB is the limit for the number of channels
|
---|
| 831 | * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
|
---|
| 832 | * @param c The char to parse
|
---|
| 833 | *
|
---|
| 834 | * @param returnMsg NULL if no message could be decoded, the message data else
|
---|
| 835 | * @param returnStats if a message was decoded, this is filled with the channel's stats
|
---|
| 836 | * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
|
---|
| 837 | *
|
---|
| 838 | * A typical use scenario of this function call is:
|
---|
| 839 | *
|
---|
| 840 | * @code
|
---|
| 841 | * #include <mavlink.h>
|
---|
| 842 | *
|
---|
| 843 | * mavlink_message_t msg;
|
---|
| 844 | * int chan = 0;
|
---|
| 845 | *
|
---|
| 846 | *
|
---|
| 847 | * while(serial.bytesAvailable > 0)
|
---|
| 848 | * {
|
---|
| 849 | * uint8_t byte = serial.getNextByte();
|
---|
| 850 | * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
|
---|
| 851 | * {
|
---|
| 852 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
|
---|
| 853 | * }
|
---|
| 854 | * }
|
---|
| 855 | *
|
---|
| 856 | *
|
---|
| 857 | * @endcode
|
---|
| 858 | */
|
---|
| 859 | MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
---|
| 860 | {
|
---|
| 861 | return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
|
---|
| 862 | mavlink_get_channel_status(chan),
|
---|
| 863 | c,
|
---|
| 864 | r_message,
|
---|
| 865 | r_mavlink_status);
|
---|
| 866 | }
|
---|
| 867 |
|
---|
| 868 | /**
|
---|
| 869 | * Set the protocol version
|
---|
| 870 | */
|
---|
| 871 | MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version)
|
---|
| 872 | {
|
---|
| 873 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 874 | if (version > 1) {
|
---|
| 875 | status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
|
---|
| 876 | } else {
|
---|
| 877 | status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
|
---|
| 878 | }
|
---|
| 879 | }
|
---|
| 880 |
|
---|
| 881 | /**
|
---|
| 882 | * Get the protocol version
|
---|
| 883 | *
|
---|
| 884 | * @return 1 for v1, 2 for v2
|
---|
| 885 | */
|
---|
| 886 | MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan, unsigned int version)
|
---|
| 887 | {
|
---|
| 888 | mavlink_status_t *status = mavlink_get_channel_status(chan);
|
---|
| 889 | if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) {
|
---|
| 890 | return 1;
|
---|
| 891 | } else {
|
---|
| 892 | return 2;
|
---|
| 893 | }
|
---|
| 894 | }
|
---|
| 895 |
|
---|
| 896 | /**
|
---|
| 897 | * This is a convenience function which handles the complete MAVLink parsing.
|
---|
| 898 | * the function will parse one byte at a time and return the complete packet once
|
---|
| 899 | * it could be successfully decoded. This function will return 0 or 1.
|
---|
| 900 | *
|
---|
| 901 | * Messages are parsed into an internal buffer (one for each channel). When a complete
|
---|
| 902 | * message is received it is copies into *returnMsg and the channel's status is
|
---|
| 903 | * copied into *returnStats.
|
---|
| 904 | *
|
---|
| 905 | * @param chan ID of the current channel. This allows to parse different channels with this function.
|
---|
| 906 | * a channel is not a physical message channel like a serial port, but a logic partition of
|
---|
| 907 | * the communication streams in this case. COMM_NB is the limit for the number of channels
|
---|
| 908 | * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
|
---|
| 909 | * @param c The char to parse
|
---|
| 910 | *
|
---|
| 911 | * @param returnMsg NULL if no message could be decoded, the message data else
|
---|
| 912 | * @param returnStats if a message was decoded, this is filled with the channel's stats
|
---|
| 913 | * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
|
---|
| 914 | *
|
---|
| 915 | * A typical use scenario of this function call is:
|
---|
| 916 | *
|
---|
| 917 | * @code
|
---|
| 918 | * #include <mavlink.h>
|
---|
| 919 | *
|
---|
| 920 | * mavlink_message_t msg;
|
---|
| 921 | * int chan = 0;
|
---|
| 922 | *
|
---|
| 923 | *
|
---|
| 924 | * while(serial.bytesAvailable > 0)
|
---|
| 925 | * {
|
---|
| 926 | * uint8_t byte = serial.getNextByte();
|
---|
| 927 | * if (mavlink_parse_char(chan, byte, &msg))
|
---|
| 928 | * {
|
---|
| 929 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
|
---|
| 930 | * }
|
---|
| 931 | * }
|
---|
| 932 | *
|
---|
| 933 | *
|
---|
| 934 | * @endcode
|
---|
| 935 | */
|
---|
| 936 | MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
---|
| 937 | {
|
---|
| 938 | uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
|
---|
| 939 | if (msg_received == MAVLINK_FRAMING_BAD_CRC ||
|
---|
| 940 | msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {
|
---|
| 941 | // we got a bad CRC. Treat as a parse failure
|
---|
| 942 | mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
|
---|
| 943 | mavlink_status_t* status = mavlink_get_channel_status(chan);
|
---|
| 944 | _mav_parse_error(status);
|
---|
| 945 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
---|
| 946 | status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
---|
| 947 | if (c == MAVLINK_STX)
|
---|
| 948 | {
|
---|
| 949 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
---|
| 950 | rxmsg->len = 0;
|
---|
| 951 | mavlink_start_checksum(rxmsg);
|
---|
| 952 | }
|
---|
| 953 | return 0;
|
---|
| 954 | }
|
---|
| 955 | return msg_received;
|
---|
| 956 | }
|
---|
| 957 |
|
---|
| 958 | /**
|
---|
| 959 | * @brief Put a bitfield of length 1-32 bit into the buffer
|
---|
| 960 | *
|
---|
| 961 | * @param b the value to add, will be encoded in the bitfield
|
---|
| 962 | * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
|
---|
| 963 | * @param packet_index the position in the packet (the index of the first byte to use)
|
---|
| 964 | * @param bit_index the position in the byte (the index of the first bit to use)
|
---|
| 965 | * @param buffer packet buffer to write into
|
---|
| 966 | * @return new position of the last used byte in the buffer
|
---|
| 967 | */
|
---|
| 968 | 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)
|
---|
| 969 | {
|
---|
| 970 | uint16_t bits_remain = bits;
|
---|
| 971 | // Transform number into network order
|
---|
| 972 | int32_t v;
|
---|
| 973 | uint8_t i_bit_index, i_byte_index, curr_bits_n;
|
---|
| 974 | #if MAVLINK_NEED_BYTE_SWAP
|
---|
| 975 | union {
|
---|
| 976 | int32_t i;
|
---|
| 977 | uint8_t b[4];
|
---|
| 978 | } bin, bout;
|
---|
| 979 | bin.i = b;
|
---|
| 980 | bout.b[0] = bin.b[3];
|
---|
| 981 | bout.b[1] = bin.b[2];
|
---|
| 982 | bout.b[2] = bin.b[1];
|
---|
| 983 | bout.b[3] = bin.b[0];
|
---|
| 984 | v = bout.i;
|
---|
| 985 | #else
|
---|
| 986 | v = b;
|
---|
| 987 | #endif
|
---|
| 988 |
|
---|
| 989 | // buffer in
|
---|
| 990 | // 01100000 01000000 00000000 11110001
|
---|
| 991 | // buffer out
|
---|
| 992 | // 11110001 00000000 01000000 01100000
|
---|
| 993 |
|
---|
| 994 | // Existing partly filled byte (four free slots)
|
---|
| 995 | // 0111xxxx
|
---|
| 996 |
|
---|
| 997 | // Mask n free bits
|
---|
| 998 | // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
|
---|
| 999 | // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
|
---|
| 1000 |
|
---|
| 1001 | // Shift n bits into the right position
|
---|
| 1002 | // out = in >> n;
|
---|
| 1003 |
|
---|
| 1004 | // Mask and shift bytes
|
---|
| 1005 | i_bit_index = bit_index;
|
---|
| 1006 | i_byte_index = packet_index;
|
---|
| 1007 | if (bit_index > 0)
|
---|
| 1008 | {
|
---|
| 1009 | // If bits were available at start, they were available
|
---|
| 1010 | // in the byte before the current index
|
---|
| 1011 | i_byte_index--;
|
---|
| 1012 | }
|
---|
| 1013 |
|
---|
| 1014 | // While bits have not been packed yet
|
---|
| 1015 | while (bits_remain > 0)
|
---|
| 1016 | {
|
---|
| 1017 | // Bits still have to be packed
|
---|
| 1018 | // there can be more than 8 bits, so
|
---|
| 1019 | // we might have to pack them into more than one byte
|
---|
| 1020 |
|
---|
| 1021 | // First pack everything we can into the current 'open' byte
|
---|
| 1022 | //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
|
---|
| 1023 | //FIXME
|
---|
| 1024 | if (bits_remain <= (uint8_t)(8 - i_bit_index))
|
---|
| 1025 | {
|
---|
| 1026 | // Enough space
|
---|
| 1027 | curr_bits_n = (uint8_t)bits_remain;
|
---|
| 1028 | }
|
---|
| 1029 | else
|
---|
| 1030 | {
|
---|
| 1031 | curr_bits_n = (8 - i_bit_index);
|
---|
| 1032 | }
|
---|
| 1033 |
|
---|
| 1034 | // Pack these n bits into the current byte
|
---|
| 1035 | // Mask out whatever was at that position with ones (xxx11111)
|
---|
| 1036 | buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
|
---|
| 1037 | // Put content to this position, by masking out the non-used part
|
---|
| 1038 | buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
|
---|
| 1039 |
|
---|
| 1040 | // Increment the bit index
|
---|
| 1041 | i_bit_index += curr_bits_n;
|
---|
| 1042 |
|
---|
| 1043 | // Now proceed to the next byte, if necessary
|
---|
| 1044 | bits_remain -= curr_bits_n;
|
---|
| 1045 | if (bits_remain > 0)
|
---|
| 1046 | {
|
---|
| 1047 | // Offer another 8 bits / one byte
|
---|
| 1048 | i_byte_index++;
|
---|
| 1049 | i_bit_index = 0;
|
---|
| 1050 | }
|
---|
| 1051 | }
|
---|
| 1052 |
|
---|
| 1053 | *r_bit_index = i_bit_index;
|
---|
| 1054 | // If a partly filled byte is present, mark this as consumed
|
---|
| 1055 | if (i_bit_index != 7) i_byte_index++;
|
---|
| 1056 | return i_byte_index - packet_index;
|
---|
| 1057 | }
|
---|
| 1058 |
|
---|
| 1059 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 1060 |
|
---|
| 1061 | // To make MAVLink work on your MCU, define comm_send_ch() if you wish
|
---|
| 1062 | // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
|
---|
| 1063 | // whole packet at a time
|
---|
| 1064 |
|
---|
| 1065 | /*
|
---|
| 1066 |
|
---|
| 1067 | #include "mavlink_types.h"
|
---|
| 1068 |
|
---|
| 1069 | void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
|
---|
| 1070 | {
|
---|
| 1071 | if (chan == MAVLINK_COMM_0)
|
---|
| 1072 | {
|
---|
| 1073 | uart0_transmit(ch);
|
---|
| 1074 | }
|
---|
| 1075 | if (chan == MAVLINK_COMM_1)
|
---|
| 1076 | {
|
---|
| 1077 | uart1_transmit(ch);
|
---|
| 1078 | }
|
---|
| 1079 | }
|
---|
| 1080 | */
|
---|
| 1081 |
|
---|
| 1082 | MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
|
---|
| 1083 | {
|
---|
| 1084 | #ifdef MAVLINK_SEND_UART_BYTES
|
---|
| 1085 | /* this is the more efficient approach, if the platform
|
---|
| 1086 | defines it */
|
---|
| 1087 | MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
|
---|
| 1088 | #else
|
---|
| 1089 | /* fallback to one byte at a time */
|
---|
| 1090 | uint16_t i;
|
---|
| 1091 | for (i = 0; i < len; i++) {
|
---|
| 1092 | comm_send_ch(chan, (uint8_t)buf[i]);
|
---|
| 1093 | }
|
---|
| 1094 | #endif
|
---|
| 1095 | }
|
---|
| 1096 | #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
---|
| 1097 |
|
---|
| 1098 | #endif /* _MAVLINK_HELPERS_H_ */
|
---|
| 1099 |
|
---|
| 1100 |
|
---|