Line data Source code
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 1 : AP_Logger_MAVLink::AP_Logger_MAVLink(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
29 : AP_Logger_Backend(front, writer),
30 1 : _max_blocks_per_send_blocks(8)
31 : {
32 1 : _blockcount = 1024*((uint8_t)_front._params.mav_bufsize) / sizeof(struct dm_block);
33 : // ::fprintf(stderr, "DM: Using %u blocks\n", _blockcount);
34 1 : }
35 :
36 : // initialisation
37 1 : void AP_Logger_MAVLink::Init()
38 : {
39 1 : _blocks = nullptr;
40 1 : while (_blockcount >= 8) { // 8 is a *magic* number
41 1 : _blocks = (struct dm_block *) calloc(_blockcount, sizeof(struct dm_block));
42 1 : if (_blocks != nullptr) {
43 1 : break;
44 : }
45 0 : _blockcount /= 2;
46 : }
47 :
48 1 : if (_blocks == nullptr) {
49 0 : return;
50 : }
51 :
52 1 : free_all_blocks();
53 1 : stats_init();
54 :
55 1 : _initialised = true;
56 : }
57 :
58 1581 : bool AP_Logger_MAVLink::logging_failed() const
59 : {
60 1581 : return !_sending_to_client;
61 : }
62 :
63 45637 : uint32_t AP_Logger_MAVLink::bufferspace_available() {
64 45637 : return (_blockcount_free * 200 + remaining_space_in_current_block());
65 : }
66 :
67 61487 : uint8_t AP_Logger_MAVLink::remaining_space_in_current_block() const {
68 : // note that _current_block *could* be NULL ATM.
69 61487 : return (MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN - _latest_block_len);
70 : }
71 :
72 6638 : void AP_Logger_MAVLink::enqueue_block(dm_block_queue_t &queue, struct dm_block *block)
73 : {
74 6638 : if (queue.youngest != nullptr) {
75 5425 : queue.youngest->next = block;
76 : } else {
77 1213 : queue.oldest = block;
78 : }
79 6638 : queue.youngest = block;
80 6638 : }
81 :
82 9662 : struct AP_Logger_MAVLink::dm_block *AP_Logger_MAVLink::dequeue_seqno(AP_Logger_MAVLink::dm_block_queue_t &queue, uint32_t seqno)
83 : {
84 9662 : struct dm_block *prev = nullptr;
85 56494 : for (struct dm_block *block=queue.oldest; block != nullptr; block=block->next) {
86 53434 : if (block->seqno == seqno) {
87 6602 : if (prev == nullptr) {
88 6602 : if (queue.youngest == queue.oldest) {
89 1212 : queue.oldest = nullptr;
90 1212 : queue.youngest = nullptr;
91 : } else {
92 5390 : queue.oldest = block->next;
93 : }
94 : } else {
95 0 : if (queue.youngest == block) {
96 0 : queue.youngest = prev;
97 : }
98 0 : prev->next = block->next;
99 : }
100 6602 : block->next = nullptr;
101 6602 : return block;
102 : }
103 46832 : prev = block;
104 : }
105 3060 : return nullptr;
106 : }
107 :
108 6343 : bool AP_Logger_MAVLink::free_seqno_from_queue(uint32_t seqno, dm_block_queue_t &queue)
109 : {
110 6343 : struct dm_block *block = dequeue_seqno(queue, seqno);
111 6343 : if (block != nullptr) {
112 3283 : block->next = _blocks_free;
113 3283 : _blocks_free = block;
114 3283 : _blockcount_free++; // comment me out to expose a bug!
115 3283 : return true;
116 : }
117 3060 : return false;
118 : }
119 :
120 :
121 192038 : bool AP_Logger_MAVLink::WritesOK() const
122 : {
123 192038 : if (!_sending_to_client) {
124 78931 : return false;
125 : }
126 113107 : 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 37556 : bool AP_Logger_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
133 : {
134 37556 : if (!semaphore.take_nonblocking()) {
135 4 : _dropped++;
136 4 : return false;
137 : }
138 :
139 37552 : if (bufferspace_available() < size) {
140 24946 : if (_startup_messagewriter->finished()) {
141 : // do not count the startup packets as being dropped...
142 21299 : _dropped++;
143 : }
144 24946 : semaphore.give();
145 24946 : return false;
146 : }
147 :
148 12607 : uint16_t copied = 0;
149 :
150 28457 : while (copied < size) {
151 15850 : if (_current_block == nullptr) {
152 17 : _current_block = next_block();
153 17 : if (_current_block == nullptr) {
154 : // should not happen - there's a sanity check above
155 0 : INTERNAL_ERROR(AP_InternalError::error_t::logger_bad_current_block);
156 0 : semaphore.give();
157 0 : return false;
158 : }
159 : }
160 15850 : uint16_t remaining_to_copy = size - copied;
161 15850 : uint16_t _curr_remaining = remaining_space_in_current_block();
162 15850 : uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
163 15850 : memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
164 15850 : copied += to_copy;
165 15850 : _latest_block_len += to_copy;
166 15850 : if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
167 : //block full, mark it to be sent:
168 3319 : enqueue_block(_blocks_pending, _current_block);
169 3319 : _current_block = next_block();
170 : }
171 : }
172 :
173 12607 : semaphore.give();
174 :
175 12607 : return true;
176 : }
177 :
178 : //Get a free block
179 3336 : struct AP_Logger_MAVLink::dm_block *AP_Logger_MAVLink::next_block()
180 : {
181 3336 : AP_Logger_MAVLink::dm_block *ret = _blocks_free;
182 3336 : if (ret != nullptr) {
183 3320 : _blocks_free = ret->next;
184 3320 : _blockcount_free--;
185 3320 : ret->seqno = _next_seq_num++;
186 3320 : ret->last_sent = 0;
187 3320 : ret->next = nullptr;
188 3320 : _latest_block_len = 0;
189 : }
190 3336 : return ret;
191 : }
192 :
193 2 : void AP_Logger_MAVLink::free_all_blocks()
194 : {
195 2 : _blocks_free = nullptr;
196 2 : _current_block = nullptr;
197 :
198 2 : _blocks_pending.sent_count = 0;
199 2 : _blocks_pending.oldest = _blocks_pending.youngest = nullptr;
200 2 : _blocks_retry.sent_count = 0;
201 2 : _blocks_retry.oldest = _blocks_retry.youngest = nullptr;
202 2 : _blocks_sent.sent_count = 0;
203 2 : _blocks_sent.oldest = _blocks_sent.youngest = nullptr;
204 :
205 : // add blocks to the free stack:
206 76 : for(uint8_t i=0; i < _blockcount; i++) {
207 74 : _blocks[i].next = _blocks_free;
208 74 : _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 74 : _blocks[i].seqno = 9876543;
214 : }
215 2 : _blockcount_free = _blockcount;
216 :
217 2 : _latest_block_len = 0;
218 2 : }
219 :
220 0 : void AP_Logger_MAVLink::stop_logging()
221 : {
222 0 : if (_sending_to_client) {
223 0 : _sending_to_client = false;
224 0 : _last_response_time = AP_HAL::millis();
225 : }
226 0 : }
227 :
228 4814 : void AP_Logger_MAVLink::handle_ack(const GCS_MAVLINK &link,
229 : const mavlink_message_t &msg,
230 : uint32_t seqno)
231 : {
232 4814 : if (!_initialised) {
233 0 : return;
234 : }
235 4814 : if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_STOP) {
236 : Debug("Received stop-logging packet");
237 0 : stop_logging();
238 0 : return;
239 : }
240 4814 : if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_START) {
241 1 : if (!_sending_to_client) {
242 : Debug("Starting New Log");
243 1 : free_all_blocks();
244 : // _current_block = next_block();
245 : // if (_current_block == nullptr) {
246 : // Debug("No free blocks?!!!\n");
247 : // return;
248 : // }
249 1 : stats_init();
250 1 : _sending_to_client = true;
251 1 : _target_system_id = msg.sysid;
252 1 : _target_component_id = msg.compid;
253 1 : _link = &link;
254 1 : _next_seq_num = 0;
255 1 : start_new_log_reset_variables();
256 1 : _last_response_time = AP_HAL::millis();
257 : Debug("Target: (%u/%u)", _target_system_id, _target_component_id);
258 : }
259 1 : return;
260 : }
261 :
262 : // check SENT blocks (VERY likely to be first on the list):
263 4813 : if (free_seqno_from_queue(seqno, _blocks_sent)) {
264 : // celebrate
265 3283 : _last_response_time = AP_HAL::millis();
266 1530 : } else if(free_seqno_from_queue(seqno, _blocks_retry)) {
267 : // party
268 0 : _last_response_time = AP_HAL::millis();
269 : } else {
270 : // probably acked already and put on the free list.
271 : }
272 : }
273 :
274 4814 : 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 4814 : mavlink_msg_remote_log_block_status_decode(&msg, &packet);
279 4814 : if (!semaphore.take_nonblocking()) {
280 0 : return;
281 : }
282 4814 : switch ((MAV_REMOTE_LOG_DATA_BLOCK_STATUSES)packet.status) {
283 0 : case MAV_REMOTE_LOG_DATA_BLOCK_NACK:
284 0 : handle_retry(packet.seqno);
285 0 : break;
286 4814 : case MAV_REMOTE_LOG_DATA_BLOCK_ACK:
287 4814 : handle_ack(link, msg, packet.seqno);
288 4814 : break;
289 : // we apparently have to handle an END enum entry, just drop it so we catch future additions
290 0 : case MAV_REMOTE_LOG_DATA_BLOCK_STATUSES_ENUM_END:
291 0 : break;
292 : }
293 4814 : semaphore.give();
294 : }
295 :
296 0 : void AP_Logger_MAVLink::handle_retry(uint32_t seqno)
297 : {
298 0 : if (!_initialised || !_sending_to_client) {
299 0 : return;
300 : }
301 :
302 0 : struct dm_block *victim = dequeue_seqno(_blocks_sent, seqno);
303 0 : if (victim != nullptr) {
304 0 : _last_response_time = AP_HAL::millis();
305 0 : enqueue_block(_blocks_retry, victim);
306 : }
307 : }
308 :
309 2 : void AP_Logger_MAVLink::stats_init() {
310 2 : _dropped = 0;
311 2 : stats.resends = 0;
312 2 : stats_reset();
313 2 : }
314 135 : void AP_Logger_MAVLink::stats_reset() {
315 135 : stats.state_free = 0;
316 135 : stats.state_free_min = -1; // unsigned wrap
317 135 : stats.state_free_max = 0;
318 135 : stats.state_pending = 0;
319 135 : stats.state_pending_min = -1; // unsigned wrap
320 135 : stats.state_pending_max = 0;
321 135 : stats.state_retry = 0;
322 135 : stats.state_retry_min = -1; // unsigned wrap
323 135 : stats.state_retry_max = 0;
324 135 : stats.state_sent = 0;
325 135 : stats.state_sent_min = -1; // unsigned wrap
326 135 : stats.state_sent_max = 0;
327 135 : stats.collection_count = 0;
328 135 : }
329 :
330 133 : void AP_Logger_MAVLink::Write_DMS(AP_Logger_MAVLink &logger_mav)
331 : {
332 133 : if (logger_mav.stats.collection_count == 0) {
333 0 : return;
334 : }
335 133 : const struct log_DMS pkt{
336 : LOG_PACKET_HEADER_INIT(LOG_DMS_MSG),
337 133 : timestamp : AP_HAL::micros64(),
338 133 : seqno : logger_mav._next_seq_num-1,
339 133 : dropped : logger_mav._dropped,
340 133 : retries : logger_mav._blocks_retry.sent_count,
341 133 : resends : logger_mav.stats.resends,
342 133 : state_free_avg : (uint8_t)(logger_mav.stats.state_free/logger_mav.stats.collection_count),
343 133 : state_free_min : logger_mav.stats.state_free_min,
344 133 : state_free_max : logger_mav.stats.state_free_max,
345 133 : state_pending_avg : (uint8_t)(logger_mav.stats.state_pending/logger_mav.stats.collection_count),
346 133 : state_pending_min : logger_mav.stats.state_pending_min,
347 133 : state_pending_max : logger_mav.stats.state_pending_max,
348 133 : state_sent_avg : (uint8_t)(logger_mav.stats.state_sent/logger_mav.stats.collection_count),
349 133 : state_sent_min : logger_mav.stats.state_sent_min,
350 133 : state_sent_max : logger_mav.stats.state_sent_max,
351 133 : };
352 133 : WriteBlock(&pkt,sizeof(pkt));
353 : }
354 :
355 134 : void AP_Logger_MAVLink::stats_log()
356 : {
357 134 : if (!_initialised) {
358 0 : return;
359 : }
360 134 : if (stats.collection_count == 0) {
361 1 : return;
362 : }
363 133 : Write_DMS(*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 133 : stats_reset();
384 : }
385 :
386 4536 : uint8_t AP_Logger_MAVLink::stack_size(struct dm_block *stack)
387 : {
388 4536 : uint8_t ret = 0;
389 45886 : for (struct dm_block *block=stack; block != nullptr; block=block->next) {
390 41350 : ret++;
391 : }
392 4536 : return ret;
393 : }
394 3402 : uint8_t AP_Logger_MAVLink::queue_size(dm_block_queue_t queue)
395 : {
396 3402 : return stack_size(queue.oldest);
397 : }
398 :
399 1134 : void AP_Logger_MAVLink::stats_collect()
400 : {
401 1134 : if (!_initialised) {
402 0 : return;
403 : }
404 1134 : if (!semaphore.take_nonblocking()) {
405 0 : return;
406 : }
407 1134 : uint8_t pending = queue_size(_blocks_pending);
408 1134 : uint8_t sent = queue_size(_blocks_sent);
409 1134 : uint8_t retry = queue_size(_blocks_retry);
410 1134 : uint8_t sfree = stack_size(_blocks_free);
411 :
412 1134 : if (sfree != _blockcount_free) {
413 0 : INTERNAL_ERROR(AP_InternalError::error_t::logger_blockcount_mismatch);
414 : }
415 1134 : semaphore.give();
416 :
417 1134 : stats.state_pending += pending;
418 1134 : stats.state_sent += sent;
419 1134 : stats.state_free += sfree;
420 1134 : stats.state_retry += retry;
421 :
422 1134 : if (pending < stats.state_pending_min) {
423 164 : stats.state_pending_min = pending;
424 : }
425 1134 : if (pending > stats.state_pending_max) {
426 95 : stats.state_pending_max = pending;
427 : }
428 1134 : if (retry < stats.state_retry_min) {
429 135 : stats.state_retry_min = retry;
430 : }
431 1134 : if (retry > stats.state_retry_max) {
432 0 : stats.state_retry_max = retry;
433 : }
434 1134 : if (sent < stats.state_sent_min) {
435 210 : stats.state_sent_min = sent;
436 : }
437 1134 : if (sent > stats.state_sent_max) {
438 120 : stats.state_sent_max = sent;
439 : }
440 1134 : if (sfree < stats.state_free_min) {
441 171 : stats.state_free_min = sfree;
442 : }
443 1134 : if (sfree > stats.state_free_max) {
444 147 : stats.state_free_max = sfree;
445 : }
446 :
447 1134 : 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 7536 : bool AP_Logger_MAVLink::send_log_blocks_from_queue(dm_block_queue_t &queue)
454 : {
455 7536 : uint8_t sent_count = 0;
456 10855 : while (queue.oldest != nullptr) {
457 3340 : if (sent_count++ > _max_blocks_per_send_blocks) {
458 21 : return false;
459 : }
460 3319 : if (! send_log_block(*queue.oldest)) {
461 0 : return false;
462 : }
463 3319 : queue.sent_count++;
464 3319 : struct AP_Logger_MAVLink::dm_block *tmp = dequeue_seqno(queue,queue.oldest->seqno);
465 3319 : if (tmp != nullptr) { // should never be nullptr
466 3319 : enqueue_block(_blocks_sent, tmp);
467 : } else {
468 0 : INTERNAL_ERROR(AP_InternalError::error_t::logger_dequeue_failure);
469 : }
470 : }
471 7515 : return true;
472 : }
473 :
474 6745 : void AP_Logger_MAVLink::push_log_blocks()
475 : {
476 6745 : if (!_initialised || !_sending_to_client) {
477 2977 : return;
478 : }
479 :
480 3768 : AP_Logger_Backend::WriteMoreStartupMessages();
481 :
482 3768 : if (!semaphore.take_nonblocking()) {
483 0 : return;
484 : }
485 :
486 3768 : if (! send_log_blocks_from_queue(_blocks_retry)) {
487 0 : semaphore.give();
488 0 : return;
489 : }
490 :
491 3768 : if (! send_log_blocks_from_queue(_blocks_pending)) {
492 21 : semaphore.give();
493 21 : return;
494 : }
495 3747 : semaphore.give();
496 : }
497 :
498 1134 : void AP_Logger_MAVLink::do_resends(uint32_t now)
499 : {
500 1134 : if (!_initialised || !_sending_to_client) {
501 501 : return;
502 : }
503 :
504 633 : uint8_t count_to_send = 5;
505 633 : if (_blockcount < count_to_send) {
506 0 : count_to_send = _blockcount;
507 : }
508 633 : uint32_t oldest = now - 100; // 100 milliseconds before resend. Hmm.
509 3798 : while (count_to_send-- > 0) {
510 3165 : if (!semaphore.take_nonblocking()) {
511 0 : return;
512 : }
513 95115 : for (struct dm_block *block=_blocks_sent.oldest; block != nullptr; block=block->next) {
514 : // only want to send blocks every now-and-then:
515 91950 : if (block->last_sent < oldest) {
516 16307 : if (! send_log_block(*block)) {
517 : // failed to send the block; try again later....
518 0 : semaphore.give();
519 0 : return;
520 : }
521 16307 : stats.resends++;
522 : }
523 : }
524 3165 : 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 1134 : void AP_Logger_MAVLink::periodic_10Hz(const uint32_t now)
532 : {
533 1134 : do_resends(now);
534 1134 : stats_collect();
535 1134 : }
536 134 : void AP_Logger_MAVLink::periodic_1Hz()
537 : {
538 135 : if (rate_limiter == nullptr &&
539 1 : (_front._params.mav_ratemax > 0 ||
540 0 : _front._params.disarm_ratemax > 0 ||
541 0 : _front._log_pause)) {
542 : // setup rate limiting if log rate max > 0Hz or log pause of streaming entries is requested
543 1 : rate_limiter = NEW_NOTHROW AP_Logger_RateLimiter(_front, _front._params.mav_ratemax, _front._params.disarm_ratemax);
544 : }
545 :
546 134 : if (_sending_to_client &&
547 75 : _last_response_time + 10000 < _last_send_time) {
548 : // other end appears to have timed out!
549 : Debug("Client timed out");
550 0 : _sending_to_client = false;
551 0 : return;
552 : }
553 134 : stats_log();
554 : }
555 :
556 : //TODO: handle full txspace properly
557 19626 : bool AP_Logger_MAVLink::send_log_block(struct dm_block &block)
558 : {
559 19626 : if (!_initialised) {
560 0 : return false;
561 : }
562 19626 : if (_link == nullptr) {
563 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
564 0 : return false;
565 : }
566 : // don't completely fill buffers - and also ensure there's enough
567 : // room to send at least one packet:
568 19626 : 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 19626 : if (_link->txspace() < min_payload_space) {
572 0 : 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 19626 : mavlink_status_t *chan_status = mavlink_get_channel_status(_link->get_chan());
583 19626 : uint8_t saved_seq = chan_status->current_tx_seq;
584 19626 : chan_status->current_tx_seq = mavlink_seq++;
585 : // Debug("Sending block (%d)", block.seqno);
586 19626 : mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid,
587 : MAV_COMP_ID_LOG,
588 : &msg,
589 19626 : _target_system_id,
590 19626 : _target_component_id,
591 : block.seqno,
592 19626 : block.buf);
593 :
594 : #if DF_MAVLINK_DISABLE_INTERRUPTS
595 : hal.scheduler->restore_interrupts(istate);
596 : #endif
597 :
598 19626 : block.last_sent = AP_HAL::millis();
599 19626 : 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 19626 : _last_send_time = AP_HAL::millis();
605 :
606 19626 : _mavlink_resend_uart(_link->get_chan(), &msg);
607 :
608 19626 : return true;
609 : }
610 : #endif
|
