libraries/AP_Logger/AP_Logger_MAVLink.cpp

   1 /*
   2    AP_Logger Remote(via MAVLink) logging
   3 */
   4
   5 #include "AP_Logger_config.h"
   6
   7 #if HAL_LOGGING_MAVLINK_ENABLED
   8
   9 #include "AP_Logger_MAVLink.h"
  10
  11 #include "LogStructure.h"
  12 #include <AP_Logger/AP_Logger.h>
  13
  14 #define REMOTE_LOG_DEBUGGING 0
  15
  16 #if REMOTE_LOG_DEBUGGING
  17 #include <stdio.h>
  18  # define Debug(fmt, args ...)  do {fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
  19 #else
  20  # define Debug(fmt, args ...)
  21 #endif
  22
  23 #include <AP_InternalError/AP_InternalError.h>
  24 #include <GCS_MAVLink/GCS.h>
  25
  26 extern const AP_HAL::HAL& hal;
  27
  28 AP_Logger_MAVLink::AP_Logger_MAVLink(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
  29     AP_Logger_Backend(front, writer),
  30     _max_blocks_per_send_blocks(8)
  31 {
  32     _blockcount = 1024*((uint8_t)_front._params.mav_bufsize) / sizeof(struct dm_block);
  33     // ::fprintf(stderr, "DM: Using %u blocks\n", _blockcount);
  34 }
  35
  36 // initialisation
  37 void AP_Logger_MAVLink::Init()
  38 {
  39     _blocks = nullptr;
  40     while (_blockcount >= 8) { // 8 is a *magic* number
  41         _blocks = (struct dm_block *) calloc(_blockcount, sizeof(struct dm_block));
  42         if (_blocks != nullptr) {
  43             break;
  44         }
  45         _blockcount /= 2;
  46     }
  47
  48     if (_blocks == nullptr) {
  49         return;
  50     }
  51
  52     free_all_blocks();
  53     stats_init();
  54
  55     _initialised = true;
  56 }
  57
  58 bool AP_Logger_MAVLink::logging_failed() const
  59 {
  60     return !_sending_to_client;
  61 }
  62
  63 uint32_t AP_Logger_MAVLink::bufferspace_available() {
  64     return (_blockcount_free * 200 + remaining_space_in_current_block());
  65 }
  66
  67 uint8_t AP_Logger_MAVLink::remaining_space_in_current_block() const {
  68     // note that _current_block *could* be NULL ATM.
  69     return (MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN - _latest_block_len);
  70 }
  71
  72 void AP_Logger_MAVLink::enqueue_block(dm_block_queue_t &queue, struct dm_block *block)
  73 {
  74     if (queue.youngest != nullptr) {
  75         queue.youngest->next = block;
  76     } else {
  77         queue.oldest = block;
  78     }
  79     queue.youngest = block;
  80 }
  81
  82 struct AP_Logger_MAVLink::dm_block *AP_Logger_MAVLink::dequeue_seqno(AP_Logger_MAVLink::dm_block_queue_t &queue, uint32_t seqno)
  83 {
  84     struct dm_block *prev = nullptr;
  85     for (struct dm_block *block=queue.oldest; block != nullptr; block=block->next) {
  86         if (block->seqno == seqno) {
  87             if (prev == nullptr) {
  88                 if (queue.youngest == queue.oldest) {
  89                     queue.oldest = nullptr;
  90                     queue.youngest = nullptr;
  91                 } else {
  92                     queue.oldest = block->next;
  93                 }
  94             } else {
  95                 if (queue.youngest == block) {
  96                     queue.youngest = prev;
  97                 }
  98                 prev->next = block->next;
  99             }
 100             block->next = nullptr;
 101             return block;
 102         }
 103         prev = block;
 104     }
 105     return nullptr;
 106 }
 107
 108 bool AP_Logger_MAVLink::free_seqno_from_queue(uint32_t seqno, dm_block_queue_t &queue)
 109 {
 110     struct dm_block *block = dequeue_seqno(queue, seqno);
 111     if (block != nullptr) {
 112         block->next = _blocks_free;
 113         _blocks_free = block;
 114         _blockcount_free++; // comment me out to expose a bug!
 115         return true;
 116     }
 117     return false;
 118 }
 119
 120
 121 bool AP_Logger_MAVLink::WritesOK() const
 122 {
 123     if (!_sending_to_client) {
 124         return false;
 125     }
 126     return true;
 127 }
 128
 129 /* Write a block of data at current offset */
 130
 131 // DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms
 132 bool AP_Logger_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
 133 {
 134     if (!semaphore.take_nonblocking()) {
 135         _dropped++;
 136         return false;
 137     }
 138
 139     if (bufferspace_available() < size) {
 140         if (_startup_messagewriter->finished()) {
 141             // do not count the startup packets as being dropped...
 142             _dropped++;
 143         }
 144         semaphore.give();
 145         return false;
 146     }
 147
 148     uint16_t copied = 0;
 149
 150     while (copied < size) {
 151         if (_current_block == nullptr) {
 152             _current_block = next_block();
 153             if (_current_block == nullptr) {
 154                 // should not happen - there's a sanity check above
 155                 INTERNAL_ERROR(AP_InternalError::error_t::logger_bad_current_block);
 156                 semaphore.give();
 157                 return false;
 158             }
 159         }
 160         uint16_t remaining_to_copy = size - copied;
 161         uint16_t _curr_remaining = remaining_space_in_current_block();
 162         uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
 163         memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
 164         copied += to_copy;
 165         _latest_block_len += to_copy;
 166         if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
 167             //block full, mark it to be sent:
 168             enqueue_block(_blocks_pending, _current_block);
 169             _current_block = next_block();
 170         }
 171     }
 172
 173     semaphore.give();
 174
 175     return true;
 176 }
 177
 178 //Get a free block
 179 struct AP_Logger_MAVLink::dm_block *AP_Logger_MAVLink::next_block()
 180 {
 181     AP_Logger_MAVLink::dm_block *ret = _blocks_free;
 182     if (ret != nullptr) {
 183         _blocks_free = ret->next;
 184         _blockcount_free--;
 185         ret->seqno = _next_seq_num++;
 186         ret->last_sent = 0;
 187         ret->next = nullptr;
 188         _latest_block_len = 0;
 189     }
 190     return ret;
 191 }
 192
 193 void AP_Logger_MAVLink::free_all_blocks()
 194 {
 195     _blocks_free = nullptr;
 196     _current_block = nullptr;
 197
 198     _blocks_pending.sent_count = 0;
 199     _blocks_pending.oldest = _blocks_pending.youngest = nullptr;
 200     _blocks_retry.sent_count = 0;
 201     _blocks_retry.oldest = _blocks_retry.youngest = nullptr;
 202     _blocks_sent.sent_count = 0;
 203     _blocks_sent.oldest = _blocks_sent.youngest = nullptr;
 204
 205     // add blocks to the free stack:
 206     for(uint8_t i=0; i < _blockcount; i++) {
 207         _blocks[i].next = _blocks_free;
 208         _blocks_free = &_blocks[i];
 209         // this value doesn't really matter, but it stops valgrind
 210         // complaining when acking blocks (we check seqno before
 211         // state).  Also, when we receive ACKs we check seqno, and we
 212         // want to ack the *real* block zero!
 213         _blocks[i].seqno = 9876543;
 214     }
 215     _blockcount_free = _blockcount;
 216
 217     _latest_block_len = 0;
 218 }
 219
 220 void AP_Logger_MAVLink::stop_logging()
 221 {
 222     if (_sending_to_client) {
 223         _sending_to_client = false;
 224         _last_response_time = AP_HAL::millis();
 225     }
 226 }
 227
 228 void AP_Logger_MAVLink::handle_ack(const GCS_MAVLINK &link,
 229                                    const mavlink_message_t &msg,
 230                                    uint32_t seqno)
 231 {
 232     if (!_initialised) {
 233         return;
 234     }
 235     if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_STOP) {
 236         Debug("Received stop-logging packet");
 237         stop_logging();
 238         return;
 239     }
 240     if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_START) {
 241         if (!_sending_to_client) {
 242             Debug("Starting New Log");
 243             free_all_blocks();
 244             // _current_block = next_block();
 245             // if (_current_block == nullptr) {
 246             //     Debug("No free blocks?!!!\n");
 247             //     return;
 248             // }
 249             stats_init();
 250             _sending_to_client = true;
 251             _target_system_id = msg.sysid;
 252             _target_component_id = msg.compid;
 253             _link = &link;
 254             _next_seq_num = 0;
 255             start_new_log_reset_variables();
 256             _last_response_time = AP_HAL::millis();
 257             Debug("Target: (%u/%u)", _target_system_id, _target_component_id);
 258         }
 259         return;
 260     }
 261
 262     // check SENT blocks (VERY likely to be first on the list):
 263     if (free_seqno_from_queue(seqno, _blocks_sent)) {
 264         // celebrate
 265         _last_response_time = AP_HAL::millis();
 266     } else if(free_seqno_from_queue(seqno, _blocks_retry)) {
 267         // party
 268         _last_response_time = AP_HAL::millis();
 269     } else {
 270         // probably acked already and put on the free list.
 271     }
 272 }
 273
 274 void AP_Logger_MAVLink::remote_log_block_status_msg(const GCS_MAVLINK &link,
 275                                                     const mavlink_message_t& msg)
 276 {
 277     mavlink_remote_log_block_status_t packet;
 278     mavlink_msg_remote_log_block_status_decode(&msg, &packet);
 279     if (!semaphore.take_nonblocking()) {
 280         return;
 281     }
 282     switch ((MAV_REMOTE_LOG_DATA_BLOCK_STATUSES)packet.status) {
 283         case MAV_REMOTE_LOG_DATA_BLOCK_NACK:
 284             handle_retry(packet.seqno);
 285             break;
 286         case MAV_REMOTE_LOG_DATA_BLOCK_ACK:
 287             handle_ack(link, msg, packet.seqno);
 288             break;
 289         // we apparently have to handle an END enum entry, just drop it so we catch future additions
 290         case MAV_REMOTE_LOG_DATA_BLOCK_STATUSES_ENUM_END:
 291             break;
 292     }
 293     semaphore.give();
 294 }
 295
 296 void AP_Logger_MAVLink::handle_retry(uint32_t seqno)
 297 {
 298     if (!_initialised || !_sending_to_client) {
 299         return;
 300     }
 301
 302     struct dm_block *victim = dequeue_seqno(_blocks_sent, seqno);
 303     if (victim != nullptr) {
 304         _last_response_time = AP_HAL::millis();
 305         enqueue_block(_blocks_retry, victim);
 306     }
 307 }
 308
 309 void AP_Logger_MAVLink::stats_init() {
 310     _dropped = 0;
 311     stats.resends = 0;
 312     stats_reset();
 313 }
 314 void AP_Logger_MAVLink::stats_reset() {
 315     stats.state_free = 0;
 316     stats.state_free_min = -1; // unsigned wrap
 317     stats.state_free_max = 0;
 318     stats.state_pending = 0;
 319     stats.state_pending_min = -1; // unsigned wrap
 320     stats.state_pending_max = 0;
 321     stats.state_retry = 0;
 322     stats.state_retry_min = -1; // unsigned wrap
 323     stats.state_retry_max = 0;
 324     stats.state_sent = 0;
 325     stats.state_sent_min = -1; // unsigned wrap
 326     stats.state_sent_max = 0;
 327     stats.collection_count = 0;
 328 }
 329
 330 void AP_Logger_MAVLink::Write_logger_MAV(AP_Logger_MAVLink &logger_mav)
 331 {
 332     if (logger_mav.stats.collection_count == 0) {
 333         return;
 334     }
 335     const struct log_MAV_Stats pkt{
 336         LOG_PACKET_HEADER_INIT(LOG_MAV_STATS),
 337         timestamp         : AP_HAL::micros64(),
 338         seqno             : logger_mav._next_seq_num-1,
 339         dropped           : logger_mav._dropped,
 340         retries           : logger_mav._blocks_retry.sent_count,
 341         resends           : logger_mav.stats.resends,
 342         state_free_avg    : (uint8_t)(logger_mav.stats.state_free/logger_mav.stats.collection_count),
 343         state_free_min    : logger_mav.stats.state_free_min,
 344         state_free_max    : logger_mav.stats.state_free_max,
 345         state_pending_avg : (uint8_t)(logger_mav.stats.state_pending/logger_mav.stats.collection_count),
 346         state_pending_min : logger_mav.stats.state_pending_min,
 347         state_pending_max : logger_mav.stats.state_pending_max,
 348         state_sent_avg    : (uint8_t)(logger_mav.stats.state_sent/logger_mav.stats.collection_count),
 349         state_sent_min    : logger_mav.stats.state_sent_min,
 350         state_sent_max    : logger_mav.stats.state_sent_max,
 351     };
 352     WriteBlock(&pkt,sizeof(pkt));
 353 }
 354
 355 void AP_Logger_MAVLink::stats_log()
 356 {
 357     if (!_initialised) {
 358         return;
 359     }
 360     if (stats.collection_count == 0) {
 361         return;
 362     }
 363     Write_logger_MAV(*this);
 364 #if REMOTE_LOG_DEBUGGING
 365     printf("D:%d Retry:%d Resent:%d SF:%d/%d/%d SP:%d/%d/%d SS:%d/%d/%d SR:%d/%d/%d\n",
 366            _dropped,
 367            _blocks_retry.sent_count,
 368            stats.resends,
 369            stats.state_free_min,
 370            stats.state_free_max,
 371            stats.state_free/stats.collection_count,
 372            stats.state_pending_min,
 373            stats.state_pending_max,
 374            stats.state_pending/stats.collection_count,
 375            stats.state_sent_min,
 376            stats.state_sent_max,
 377            stats.state_sent/stats.collection_count,
 378            stats.state_retry_min,
 379            stats.state_retry_max,
 380            stats.state_retry/stats.collection_count
 381         );
 382 #endif
 383     stats_reset();
 384 }
 385
 386 uint8_t AP_Logger_MAVLink::stack_size(struct dm_block *stack)
 387 {
 388     uint8_t ret = 0;
 389     for (struct dm_block *block=stack; block != nullptr; block=block->next) {
 390         ret++;
 391     }
 392     return ret;
 393 }
 394 uint8_t AP_Logger_MAVLink::queue_size(dm_block_queue_t queue)
 395 {
 396     return stack_size(queue.oldest);
 397 }
 398
 399 void AP_Logger_MAVLink::stats_collect()
 400 {
 401     if (!_initialised) {
 402         return;
 403     }
 404     if (!semaphore.take_nonblocking()) {
 405         return;
 406     }
 407     uint8_t pending = queue_size(_blocks_pending);
 408     uint8_t sent = queue_size(_blocks_sent);
 409     uint8_t retry = queue_size(_blocks_retry);
 410     uint8_t sfree = stack_size(_blocks_free);
 411
 412     if (sfree != _blockcount_free) {
 413         INTERNAL_ERROR(AP_InternalError::error_t::logger_blockcount_mismatch);
 414     }
 415     semaphore.give();
 416
 417     stats.state_pending += pending;
 418     stats.state_sent += sent;
 419     stats.state_free += sfree;
 420     stats.state_retry += retry;
 421
 422     if (pending < stats.state_pending_min) {
 423         stats.state_pending_min = pending;
 424     }
 425     if (pending > stats.state_pending_max) {
 426         stats.state_pending_max = pending;
 427     }
 428     if (retry < stats.state_retry_min) {
 429         stats.state_retry_min = retry;
 430     }
 431     if (retry > stats.state_retry_max) {
 432         stats.state_retry_max = retry;
 433     }
 434     if (sent < stats.state_sent_min) {
 435         stats.state_sent_min = sent;
 436     }
 437     if (sent > stats.state_sent_max) {
 438         stats.state_sent_max = sent;
 439     }
 440     if (sfree < stats.state_free_min) {
 441         stats.state_free_min = sfree;
 442     }
 443     if (sfree > stats.state_free_max) {
 444         stats.state_free_max = sfree;
 445     }
 446
 447     stats.collection_count++;
 448 }
 449
 450 /* while we "successfully" send log blocks from a queue, move them to
 451  * the sent list. DO NOT call this for blocks already sent!
 452 */
 453 bool AP_Logger_MAVLink::send_log_blocks_from_queue(dm_block_queue_t &queue)
 454 {
 455     uint8_t sent_count = 0;
 456     while (queue.oldest != nullptr) {
 457         if (sent_count++ > _max_blocks_per_send_blocks) {
 458             return false;
 459         }
 460         if (! send_log_block(*queue.oldest)) {
 461             return false;
 462         }
 463         queue.sent_count++;
 464         struct AP_Logger_MAVLink::dm_block *tmp = dequeue_seqno(queue,queue.oldest->seqno);
 465         if (tmp != nullptr) { // should never be nullptr
 466             enqueue_block(_blocks_sent, tmp);
 467         } else {
 468             INTERNAL_ERROR(AP_InternalError::error_t::logger_dequeue_failure);
 469         }
 470     }
 471     return true;
 472 }
 473
 474 void AP_Logger_MAVLink::push_log_blocks()
 475 {
 476     if (!_initialised || !_sending_to_client) {
 477         return;
 478     }
 479
 480     AP_Logger_Backend::WriteMoreStartupMessages();
 481
 482     if (!semaphore.take_nonblocking()) {
 483         return;
 484     }
 485
 486     if (! send_log_blocks_from_queue(_blocks_retry)) {
 487         semaphore.give();
 488         return;
 489     }
 490
 491     if (! send_log_blocks_from_queue(_blocks_pending)) {
 492         semaphore.give();
 493         return;
 494     }
 495     semaphore.give();
 496 }
 497
 498 void AP_Logger_MAVLink::do_resends(uint32_t now)
 499 {
 500     if (!_initialised || !_sending_to_client) {
 501         return;
 502     }
 503
 504     uint8_t count_to_send = 5;
 505     if (_blockcount < count_to_send) {
 506         count_to_send = _blockcount;
 507     }
 508     uint32_t oldest = now - 100; // 100 milliseconds before resend.  Hmm.
 509     while (count_to_send-- > 0) {
 510         if (!semaphore.take_nonblocking()) {
 511             return;
 512         }
 513         for (struct dm_block *block=_blocks_sent.oldest; block != nullptr; block=block->next) {
 514             // only want to send blocks every now-and-then:
 515             if (block->last_sent < oldest) {
 516                 if (! send_log_block(*block)) {
 517                     // failed to send the block; try again later....
 518                     semaphore.give();
 519                     return;
 520                 }
 521                 stats.resends++;
 522             }
 523         }
 524         semaphore.give();
 525     }
 526 }
 527
 528 // NOTE: any functions called from these periodic functions MUST
 529 // handle locking of the blocks structures by taking the semaphore
 530 // appropriately!
 531 void AP_Logger_MAVLink::periodic_10Hz(const uint32_t now)
 532 {
 533     do_resends(now);
 534     stats_collect();
 535 }
 536 void AP_Logger_MAVLink::periodic_1Hz()
 537 {
 538     if (rate_limiter == nullptr &&
 539         (_front._params.mav_ratemax > 0 ||
 540          _front._params.disarm_ratemax > 0 ||
 541          _front._log_pause)) {
 542         // setup rate limiting if log rate max > 0Hz or log pause of streaming entries is requested
 543         rate_limiter = NEW_NOTHROW AP_Logger_RateLimiter(_front, _front._params.mav_ratemax, _front._params.disarm_ratemax);
 544     }
 545
 546     if (_sending_to_client &&
 547         _last_response_time + 10000 < _last_send_time) {
 548         // other end appears to have timed out!
 549         Debug("Client timed out");
 550         _sending_to_client = false;
 551         return;
 552     }
 553     stats_log();
 554 }
 555
 556 //TODO: handle full txspace properly
 557 bool AP_Logger_MAVLink::send_log_block(struct dm_block &block)
 558 {
 559     if (!_initialised) {
 560        return false;
 561     }
 562     if (_link == nullptr) {
 563         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
 564         return false;
 565     }
 566     // don't completely fill buffers - and also ensure there's enough
 567     // room to send at least one packet:
 568     const uint16_t min_payload_space = 500;
 569     static_assert(MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN <= min_payload_space,
 570                   "minimum allocated space is less than payload length");
 571     if (_link->txspace() < min_payload_space) {
 572         return false;
 573     }
 574
 575 #if DF_MAVLINK_DISABLE_INTERRUPTS
 576     void *istate = hal.scheduler->disable_interrupts_save();
 577 #endif
 578
 579 // DM_packing: 267039 events, 0 overruns, 8440834us elapsed, 31us avg, min 31us max 32us 0.488us rms
 580
 581     mavlink_message_t msg;
 582     mavlink_status_t *chan_status = mavlink_get_channel_status(_link->get_chan());
 583     uint8_t saved_seq = chan_status->current_tx_seq;
 584     chan_status->current_tx_seq = mavlink_seq++;
 585     // Debug("Sending block (%d)", block.seqno);
 586     mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid,
 587                                            MAV_COMP_ID_LOG,
 588                                            &msg,
 589                                            _target_system_id,
 590                                            _target_component_id,
 591                                            block.seqno,
 592                                            block.buf);
 593
 594 #if DF_MAVLINK_DISABLE_INTERRUPTS
 595     hal.scheduler->restore_interrupts(istate);
 596 #endif
 597
 598     block.last_sent = AP_HAL::millis();
 599     chan_status->current_tx_seq = saved_seq;
 600
 601     // _last_send_time is set even if we fail to send the packet; if
 602     // the txspace is repeatedly chockas we should not add to the
 603     // problem and stop attempting to log
 604     _last_send_time = AP_HAL::millis();
 605
 606     _mavlink_resend_uart(_link->get_chan(), &msg);
 607
 608     return true;
 609 }
 610 #endif