DataFlash: DataFlash-over-MAVLink support

libraries/DataFlash/DataFlash.cpp

@@ -5,8 +5,8 @@
 const AP_Param::GroupInfo DataFlash_Class::var_info[] = {
     // @Param: _BACKEND_TYPES
     // @DisplayName: DataFlash Backend Storage type
-    // @Description: 0 for None, 1 for File
+    // @Description: 0 for None, 1 for File, 2 for dataflash mavlink, 3 for both file and dataflash
-    // @Values: 0:None,1:File
+    // @Values: 0:None,1:File,2:MAVLink,3:BothFileAndMAVLink
     // @User: Standard
     AP_GROUPINFO("_BACKEND_TYPE",  0, DataFlash_Class, _params.backend_types,       DATAFLASH_BACKEND_FILE),
     AP_GROUPEND
@@ -158,6 +158,13 @@ void DataFlash_Class::EnableWrites(bool enable) {
     FOR_EACH_BACKEND(EnableWrites(enable));
 }
 
+// for DataFlash_MAVLink
+void DataFlash_Class::remote_log_block_status_msg(mavlink_channel_t chan,
+                                                  mavlink_message_t* msg) {
+    FOR_EACH_BACKEND(remote_log_block_status_msg(chan, msg));
+}
+// end for DataFlash_MAVLink
+
 void DataFlash_Class::periodic_tasks() {
      FOR_EACH_BACKEND(periodic_tasks());
 }

libraries/DataFlash/DataFlash.h

@@ -34,6 +34,8 @@ class DataFlash_Backend;
 enum DataFlash_Backend_Type {
     DATAFLASH_BACKEND_NONE = 0,
     DATAFLASH_BACKEND_FILE = 1,
+    DATAFLASH_BACKEND_MAVLINK = 2,
+    DATAFLASH_BACKEND_BOTH = 3,
 };
 
 class DataFlash_Class
@@ -143,6 +145,11 @@ public:
     void flush(void);
 #endif
 
+    // for DataFlash_MAVLink:
+    void remote_log_block_status_msg(mavlink_channel_t chan,
+                                     mavlink_message_t* msg);
+    // end for DataFlash_MAVLink:
+
     void periodic_tasks(); // may want to split this into GCS/non-GCS duties
 
     // this is out here for the trickle-startup-messages logging.

libraries/DataFlash/DataFlash_Backend.h

@@ -76,6 +76,11 @@ public:
     virtual void flush(void) { }
 #endif
 
+     // for Dataflash_MAVlink
+    virtual void remote_log_block_status_msg(mavlink_channel_t chan,
+                                             mavlink_message_t* msg) { }
+    // end for Dataflash_MAVlink
+
    virtual void periodic_tasks();
 
     uint8_t num_types() const;

libraries/DataFlash/DataFlash_MAVLink.cpp

@@ -0,0 +1,574 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/* 
+   DataFlash Remote(via MAVLink) logging
+*/
+
+#include "DataFlash_MAVLink.h"
+
+#if DATAFLASH_MAVLINK_SUPPORT
+
+#include "LogStructure.h"
+
+#define REMOTE_LOG_DEBUGGING 0
+
+#if REMOTE_LOG_DEBUGGING
+#include <stdio.h>
+ # define Debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#else
+ # define Debug(fmt, args ...)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+
+// initialisation
+void DataFlash_MAVLink::Init()
+{
+    DataFlash_Backend::Init();
+
+    _blocks = NULL;
+    while (_blockcount >= 8) { // 8 is a *magic* number
+        _blocks = (struct dm_block *) malloc(_blockcount * sizeof(_blocks[0]));
+        if (_blocks != NULL) {
+            break;
+        }
+        _blockcount /= 2;
+    }
+
+    if (_blocks == NULL) {
+        return;
+    }
+
+    free_all_blocks();
+    stats_init();
+
+    _initialised = true;
+    _logging_started = true; // in actual fact, we throw away
+                             // everything until a client connects.
+                             // This stops calls to start_new_log from
+                             // the vehicles
+}
+
+uint16_t DataFlash_MAVLink::bufferspace_available() {
+    return (_blockcount_free * 200 + remaining_space_in_current_block());
+}
+
+uint8_t DataFlash_MAVLink::remaining_space_in_current_block() {
+    // note that _current_block *could* be NULL ATM.
+    return (MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN - _latest_block_len);
+}
+
+void DataFlash_MAVLink::enqueue_block(dm_block_queue_t &queue, struct dm_block *block)
+{
+    if (queue.youngest != NULL) {
+        queue.youngest->next = block;
+    } else {
+        queue.oldest = block;
+    }
+    queue.youngest = block;
+}
+
+struct DataFlash_MAVLink::dm_block *DataFlash_MAVLink::dequeue_seqno(DataFlash_MAVLink::dm_block_queue_t &queue, uint32_t seqno)
+{
+    struct dm_block *prev = NULL;
+    for (struct dm_block *block=queue.oldest; block != NULL; block=block->next) {
+        if (block->seqno == seqno) {
+            if (prev == NULL) {
+                if (queue.youngest == queue.oldest) {
+                    queue.oldest = NULL;
+                    queue.youngest = NULL;
+                } else {
+                    queue.oldest = block->next;
+                }
+            } else {
+                if (queue.youngest == block) {
+                    queue.youngest = prev;
+                }
+                prev->next = block->next;
+            }
+            block->next = NULL;
+            return block;
+        }
+        prev = block;
+    }
+    return NULL;
+}
+
+bool DataFlash_MAVLink::free_seqno_from_queue(uint32_t seqno, dm_block_queue_t &queue)
+{
+    struct dm_block *block = dequeue_seqno(queue, seqno);
+    if (block != NULL) {
+        block->next = _blocks_free;
+        _blocks_free = block;
+        _blockcount_free++; // comment me out to expose a bug!
+        return true;
+    }
+    return false;
+}
+    
+/* Write a block of data at current offset */
+
+// DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms
+bool DataFlash_MAVLink::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
+{
+    if (!_initialised || !_sending_to_client || !_writes_enabled) {
+        return false;
+    }
+
+    if (! WriteBlockCheckStartupMessages()) {
+        return false;
+    }
+
+    if (bufferspace_available() < size) {
+        if (_startup_messagewriter->finished()) {
+            // do not count the startup packets as being dropped...
+            dropped++;
+        }
+        return false;
+    }
+
+    uint16_t copied = 0;
+
+    while (copied < size) {
+        if (_current_block == NULL) {
+            _current_block = next_block();
+            if (_current_block == NULL) {
+                // should not happen - there's a sanity check above
+                internal_error();
+                return false;
+            }
+        }
+        uint16_t remaining_to_copy = size - copied;
+        uint16_t _curr_remaining = remaining_space_in_current_block();
+        uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
+        memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
+        copied += to_copy;
+        _latest_block_len += to_copy;
+        if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
+            //block full, mark it to be sent:
+            enqueue_block(_blocks_pending, _current_block);
+            _current_block = next_block();
+        }
+    }
+
+    if (!_writing_startup_messages) {
+        // push_log_blocks();
+    }
+
+    return true;
+}
+
+//Get a free block
+struct DataFlash_MAVLink::dm_block *DataFlash_MAVLink::next_block()
+{
+    DataFlash_MAVLink::dm_block *ret = _blocks_free;
+    if (ret != NULL) {
+        _blocks_free = ret->next;
+        _blockcount_free--;
+        ret->seqno = _next_seq_num++;
+        ret->last_sent = 0;
+        ret->next = NULL;
+        _latest_block_len = 0;
+    }
+    return ret;
+}
+
+void DataFlash_MAVLink::free_all_blocks()
+{
+    _blocks_free = NULL;
+    _current_block = NULL;
+
+    _blocks_pending.sent_count = 0;
+    _blocks_pending.oldest = _blocks_pending.youngest = NULL;
+    _blocks_retry.sent_count = 0;
+    _blocks_retry.oldest = _blocks_retry.youngest = NULL;
+    _blocks_sent.sent_count = 0;
+    _blocks_sent.oldest = _blocks_sent.youngest = NULL;
+
+    // add blocks to the free stack:
+    for(uint8_t i=0; i < _blockcount; i++) {
+        _blocks[i].next = _blocks_free;
+        _blocks_free = &_blocks[i];
+        // this value doesn't really matter, but it stops valgrind
+        // complaining when acking blocks (we check seqno before
+        // state).  Also, when we receive ACKs we check seqno, and we
+        // want to ack the *real* block zero!
+        _blocks[i].seqno = 9876543;
+    }
+    _blockcount_free = _blockcount;
+
+    _latest_block_len = 0;
+}
+
+void DataFlash_MAVLink::handle_ack(mavlink_channel_t chan,
+                                   mavlink_message_t* msg,
+                                   uint32_t seqno)
+{
+    if (!_initialised) {
+        return;
+    }
+    if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_STOP) {
+        Debug("Received stop-logging packet");
+        if (_sending_to_client) {
+            _sending_to_client = false;
+            _last_response_time = AP_HAL::millis();
+        }
+        return;
+    }
+    if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_START) {
+        if (!_sending_to_client) {
+            Debug("Starting New Log");
+            free_all_blocks();
+            // _current_block = next_block();
+            // if (_current_block == NULL) {
+            //     Debug("No free blocks?!!!\n");
+            //     return;
+            // }
+            stats_init();
+            _sending_to_client = true;
+            _target_system_id = msg->sysid;
+            _target_component_id = msg->compid;
+            _chan = chan;
+            _next_seq_num = 0;
+            _startup_messagewriter->reset();
+            _last_response_time = AP_HAL::millis();
+            Debug("Target: (%u/%u)", _target_system_id, _target_component_id);
+        }
+        return;
+    }
+
+    // check SENT blocks (VERY likely to be first on the list):
+    if (free_seqno_from_queue(seqno, _blocks_sent)) {
+        // celebrate
+        _last_response_time = AP_HAL::millis();
+    } else if(free_seqno_from_queue(seqno, _blocks_retry)) {
+        // party
+        _last_response_time = AP_HAL::millis();
+    } else {
+        // probably acked already and put on the free list.
+    }
+}
+
+void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan,
+                                                    mavlink_message_t* msg)
+{
+    mavlink_remote_log_block_status_t packet;
+    mavlink_msg_remote_log_block_status_decode(msg, &packet);
+    if(packet.status == 0){
+        handle_retry(packet.seqno);
+    } else{
+        handle_ack(chan, msg, packet.seqno);
+    }
+}
+
+void DataFlash_MAVLink::handle_retry(uint32_t seqno)
+{
+    if (!_initialised || !_sending_to_client) {
+        return;
+    }
+
+    struct dm_block *victim = dequeue_seqno(_blocks_sent, seqno);
+    if (victim != NULL) {
+        _last_response_time = AP_HAL::millis();
+        enqueue_block(_blocks_retry, victim);
+    }
+}
+
+void DataFlash_MAVLink::set_channel(mavlink_channel_t chan)
+{
+    _chan = chan;
+}
+
+void DataFlash_MAVLink::internal_error() {
+    internal_errors++;
+    DataFlash_Backend::internal_error();
+}
+void DataFlash_MAVLink::stats_init() {
+    dropped = 0;
+    internal_errors = 0;
+    stats.resends = 0;
+    stats_reset();
+}
+void DataFlash_MAVLink::stats_reset() {
+    stats.state_free = 0;
+    stats.state_free_min = -1; // unsigned wrap
+    stats.state_free_max = 0;
+    stats.state_pending = 0;
+    stats.state_pending_min = -1; // unsigned wrap
+    stats.state_pending_max = 0;
+    stats.state_retry = 0;
+    stats.state_retry_min = -1; // unsigned wrap
+    stats.state_retry_max = 0;
+    stats.state_sent = 0;
+    stats.state_sent_min = -1; // unsigned wrap
+    stats.state_sent_max = 0;
+    stats.collection_count = 0;
+}
+
+void DataFlash_MAVLink::Log_Write_DF_MAV(DataFlash_MAVLink &df)
+{
+    if (df.stats.collection_count == 0) {
+        return;
+    }
+    struct log_DF_MAV_Stats pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_DF_MAV_STATS),
+        timestamp         : AP_HAL::millis(),
+        seqno             : df._next_seq_num-1,
+        dropped           : df.dropped,
+        retries           : df._blocks_retry.sent_count,
+        resends           : df.stats.resends,
+        internal_errors   : df.internal_errors,
+        state_free_avg    : (uint8_t)(df.stats.state_free/df.stats.collection_count),
+        state_free_min    : df.stats.state_free_min,
+        state_free_max    : df.stats.state_free_max,
+        state_pending_avg : (uint8_t)(df.stats.state_pending/df.stats.collection_count),
+        state_pending_min : df.stats.state_pending_min,
+        state_pending_max : df.stats.state_pending_max,
+        state_sent_avg    : (uint8_t)(df.stats.state_sent/df.stats.collection_count),
+        state_sent_min    : df.stats.state_sent_min,
+        state_sent_max    : df.stats.state_sent_max,
+        // state_retry_avg   : (uint8_t)(df.stats.state_retry/df.stats.collection_count),
+        // state_retry_min    : df.stats.state_retry_min,
+        // state_retry_max    : df.stats.state_retry_max
+    };
+    WriteBlock(&pkt,sizeof(pkt));
+}
+
+void DataFlash_MAVLink::stats_log()
+{
+    if (!_initialised || !_logging_started) {
+        return;
+    }
+    if (stats.collection_count == 0) {
+        return;
+    }
+    Log_Write_DF_MAV(*this);
+#if REMOTE_LOG_DEBUGGING
+    printf("D:%d Retry:%d Resent:%d E:%d SF:%d/%d/%d SP:%d/%d/%d SS:%d/%d/%d SR:%d/%d/%d\n",
+           dropped,
+           _blocks_retry.sent_count,
+           stats.resends,
+           internal_errors,
+           stats.state_free_min,
+           stats.state_free_max,
+           stats.state_free/stats.collection_count,
+           stats.state_pending_min,
+           stats.state_pending_max,
+           stats.state_pending/stats.collection_count,
+           stats.state_sent_min,
+           stats.state_sent_max,
+           stats.state_sent/stats.collection_count,
+           stats.state_retry_min,
+           stats.state_retry_max,
+           stats.state_retry/stats.collection_count
+        );
+#endif
+    stats_reset();
+}
+
+uint8_t DataFlash_MAVLink::stack_size(struct dm_block *stack)
+{
+    uint8_t ret = 0;
+    for (struct dm_block *block=stack; block != NULL; block=block->next) {
+        ret++;
+    }
+    return ret;
+}
+uint8_t DataFlash_MAVLink::queue_size(dm_block_queue_t queue)
+{
+    return stack_size(queue.oldest);
+}
+
+void DataFlash_MAVLink::stats_collect()
+{
+    if (!_initialised || !_logging_started) {
+        return;
+    }
+    uint8_t pending = queue_size(_blocks_pending);
+    uint8_t sent = queue_size(_blocks_sent);
+    uint8_t retry = queue_size(_blocks_retry);
+    uint8_t sfree = stack_size(_blocks_free);
+
+    if (sfree != _blockcount_free) {
+        internal_error();
+    }
+    stats.state_pending += pending;
+    stats.state_sent += sent;
+    stats.state_free += sfree;
+    stats.state_retry += retry;
+
+    if (pending < stats.state_pending_min) {
+        stats.state_pending_min = pending;
+    }
+    if (pending > stats.state_pending_max) {
+        stats.state_pending_max = pending;
+    }
+    if (retry < stats.state_retry_min) {
+        stats.state_retry_min = retry;
+    }
+    if (retry > stats.state_retry_max) {
+        stats.state_retry_max = retry;
+    }
+    if (sent < stats.state_sent_min) {
+        stats.state_sent_min = sent;
+    }
+    if (sent > stats.state_sent_max) {
+        stats.state_sent_max = sent;
+    }
+    if (sfree < stats.state_free_min) {
+        stats.state_free_min = sfree;
+    }
+    if (sfree > stats.state_free_max) {
+        stats.state_free_max = sfree;
+    }
+    
+    stats.collection_count++;
+}
+
+/* while we "successfully" send log blocks from a queue, move them to
+ * the sent list. DO NOT call this for blocks already sent!
+*/
+bool DataFlash_MAVLink::send_log_blocks_from_queue(dm_block_queue_t &queue)
+{
+    uint8_t sent_count = 0;
+    while (queue.oldest != NULL) {
+        if (sent_count++ > _max_blocks_per_send_blocks) {
+            return false;
+        }
+        if (! send_log_block(*queue.oldest)) {
+            return false;
+        }
+        queue.sent_count++;
+        struct DataFlash_MAVLink::dm_block *tmp = dequeue_seqno(queue,queue.oldest->seqno);
+        if (tmp != NULL) { // should never be NULL
+            enqueue_block(_blocks_sent, tmp);
+        } else {
+            internal_error();
+        }
+    }
+    return true;
+}
+
+void DataFlash_MAVLink::push_log_blocks()
+{
+    if (!_initialised || !_logging_started ||!_sending_to_client) {
+        return;
+    }
+
+    DataFlash_Backend::WriteMoreStartupMessages();
+
+    if (! send_log_blocks_from_queue(_blocks_retry)) {
+        return;
+    }
+
+    if (! send_log_blocks_from_queue(_blocks_pending)) {
+        return;
+    }
+}
+
+void DataFlash_MAVLink::do_resends(uint32_t now)
+{
+    if (!_initialised || !_logging_started ||!_sending_to_client) {
+        return;
+    }
+
+    uint8_t count_to_send = 5;
+    if (_blockcount < count_to_send) {
+        count_to_send = _blockcount;
+    }
+    uint32_t oldest = now - 100; // 100 milliseconds before resend.  Hmm.
+    while (count_to_send-- > 0) {
+        for (struct dm_block *block=_blocks_sent.oldest; block != NULL; block=block->next) {
+            // only want to send blocks every now-and-then:
+            if (block->last_sent < oldest) {
+                if (! send_log_block(*block)) {
+                    // failed to send the block; try again later....
+                    return;
+                }
+                stats.resends++;
+            }
+        }
+    }
+}
+
+void DataFlash_MAVLink::periodic_10Hz(const uint32_t now)
+{
+    do_resends(now);
+    stats_collect();
+}
+void DataFlash_MAVLink::periodic_1Hz(const uint32_t now)
+{
+    if (_sending_to_client &&
+        _last_response_time + 10000 < _last_send_time) {
+        // other end appears to have timed out!
+        Debug("Client timed out");
+        _sending_to_client = false;
+        return;
+    }
+    stats_log();
+}
+
+void DataFlash_MAVLink::periodic_fullrate(uint32_t now)
+{
+    push_log_blocks();
+}
+
+//TODO: handle full txspace properly
+bool DataFlash_MAVLink::send_log_block(struct dm_block &block)
+{
+    mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0);
+    if (!_initialised) {
+       return false;
+    }
+    if (comm_get_txspace(chan) < MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN) {
+        return false;
+    }
+    if (comm_get_txspace(chan) < 500) {
+        return false;
+    }
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+    if (rand() < 0.1) {
+        return false;
+    }
+#endif
+    
+#if DF_MAVLINK_DISABLE_INTERRUPTS
+    irqstate_t istate = irqsave();
+#endif
+
+// DM_packing: 267039 events, 0 overruns, 8440834us elapsed, 31us avg, min 31us max 32us 0.488us rms
+    hal.util->perf_begin(_perf_packing);
+
+    mavlink_message_t msg;
+    mavlink_status_t *chan_status = mavlink_get_channel_status(chan);
+    uint8_t saved_seq = chan_status->current_tx_seq;
+    chan_status->current_tx_seq = mavlink_seq++;
+    // Debug("Sending block (%d)", block.seqno);
+    mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid,
+                                           MAV_COMP_ID_LOG,
+                                           &msg,
+                                           _target_system_id,
+                                           _target_component_id,
+                                           block.seqno,
+                                           block.buf);
+
+    hal.util->perf_end(_perf_packing);
+
+#if DF_MAVLINK_DISABLE_INTERRUPTS
+    irqrestore(istate);
+#endif
+
+    block.last_sent = AP_HAL::millis();
+    chan_status->current_tx_seq = saved_seq;
+
+    // _last_send_time is set even if we fail to send the packet; if
+    // the txspace is repeatedly chockas we should not add to the
+    // problem and stop attempting to log
+    _last_send_time = AP_HAL::millis();
+
+    _mavlink_resend_uart(chan, &msg);
+
+    return true;
+}
+#endif

libraries/DataFlash/DataFlash_MAVLink.h

@@ -0,0 +1,197 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/* 
+   DataFlash logging - MAVLink variant
+
+   - transfers blocks of the open log file to a client using MAVLink
+ */
+
+#ifndef DATAFLASH_MAVLINK_H
+#define DATAFLASH_MAVLINK_H
+
+#define DATAFLASH_MAVLINK_SUPPORT 1
+
+#if DATAFLASH_MAVLINK_SUPPORT
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "DataFlash_Backend.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define DF_MAVLINK_DISABLE_INTERRUPTS 0
+
+class DataFlash_MAVLink : public DataFlash_Backend
+{
+    friend class DataFlash_Class; // for access to stats on Log_Df_Mav_Stats
+public:
+    // constructor
+    DataFlash_MAVLink(DataFlash_Class &front, DFMessageWriter_DFLogStart *writer) :
+        DataFlash_Backend(front, writer),
+        _max_blocks_per_send_blocks(8),
+        _blockcount(32) // this may get reduced in Init if allocation fails
+        ,_perf_packing(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "DM_packing"))
+        { }
+
+    // initialisation
+    void Init() override;
+
+    bool logging_started() { return _logging_started; }
+
+    /* Write a block of data at current offset */
+    bool WritePrioritisedBlock(const void *pBuffer, uint16_t size,
+                               bool is_critical) override;
+
+    // initialisation
+    bool CardInserted(void) override { return true; }
+
+    // erase handling
+    void EraseAll() override {}
+
+    bool NeedPrep() override { return false; }
+    void Prep() override { }
+
+    // high level interface
+    uint16_t find_last_log(void) override { return 0; }
+    void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) override {}
+    void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) override {}
+    int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override { return 0; }
+    uint16_t get_num_logs(void) override { return 0; }
+    
+    void LogReadProcess(uint16_t log_num,
+                        uint16_t start_page, uint16_t end_page, 
+                        print_mode_fn printMode,
+                        AP_HAL::BetterStream *port) override {}
+    void DumpPageInfo(AP_HAL::BetterStream *port) override {}
+    void ShowDeviceInfo(AP_HAL::BetterStream *port) override {}
+    void ListAvailableLogs(AP_HAL::BetterStream *port) override {}
+
+    // enum dm_block_state {
+    //     BLOCK_STATE_FREE = 17,
+    //     BLOCK_STATE_FILLING,
+    //     BLOCK_STATE_SEND_PENDING,
+    //     BLOCK_STATE_SEND_RETRY,
+    //     BLOCK_STATE_SENT
+    // };
+    struct dm_block {
+        uint32_t seqno;
+        uint8_t buf[MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN];
+        uint32_t last_sent;
+        struct dm_block *next;
+    };
+    void push_log_blocks();
+    virtual bool send_log_block(struct dm_block &block);
+    virtual void handle_ack(mavlink_channel_t chan, mavlink_message_t* msg, uint32_t seqno);
+    virtual void handle_retry(uint32_t block_num);
+    void do_resends(uint32_t now);
+    virtual void set_channel(mavlink_channel_t chan);
+    virtual void remote_log_block_status_msg(mavlink_channel_t chan, mavlink_message_t* msg) override;
+    void free_all_blocks();
+
+    // a stack for free blocks, queues for pending, sent, retries and sent
+    struct dm_block_queue {
+        uint32_t sent_count;
+        struct dm_block *oldest;
+        struct dm_block *youngest;
+    };
+    typedef struct dm_block_queue dm_block_queue_t ;
+    void enqueue_block(dm_block_queue_t &queue, struct dm_block *block);
+    bool queue_has_block(dm_block_queue_t &queue, struct dm_block *block);
+    struct dm_block *dequeue_seqno(dm_block_queue_t &queue, uint32_t seqno);
+    bool free_seqno_from_queue(uint32_t seqno, dm_block_queue_t &queue);
+    bool send_log_blocks_from_queue(dm_block_queue_t &queue);
+    uint8_t stack_size(struct dm_block *stack);
+    uint8_t queue_size(dm_block_queue_t queue);
+    
+    struct dm_block *_blocks_free;
+    dm_block_queue_t _blocks_sent;
+    dm_block_queue_t _blocks_pending;
+    dm_block_queue_t _blocks_retry;
+
+protected:
+    struct _stats {
+        // the following are reset any time we log stats (see "reset_stats")
+        uint32_t resends;
+        uint8_t collection_count;
+        uint16_t state_free; // cumulative across collection period
+        uint8_t state_free_min;
+        uint8_t state_free_max;
+        uint16_t state_pending; // cumulative across collection period
+        uint8_t state_pending_min;
+        uint8_t state_pending_max;
+        uint16_t state_retry; // cumulative across collection period
+        uint8_t state_retry_min;
+        uint8_t state_retry_max;
+        uint16_t state_sent; // cumulative across collection period
+        uint8_t state_sent_min;
+        uint8_t state_sent_max;
+    } stats;
+
+private:
+    mavlink_channel_t _chan;
+    uint8_t _target_system_id;
+    uint8_t _target_component_id;
+
+    bool _initialised;
+
+    // this controls the maximum number of blocks we will push from
+    // the pending and send queues in any call to push_log_blocks.
+    // push_log_blocks is called by periodic_tasks.  Each block is 200
+    // bytes.  In Plane, at 50Hz, a _max_blocks_per_send_blocks of 2
+    // means we will push at most 2*50*200 == 20KB of logs per second
+    // _max_blocks_per_send_blocks has to be high enough to push all
+    // of the logs, but low enough that we don't spend way too much
+    // time packing messages in any one loop
+    const uint8_t _max_blocks_per_send_blocks;
+    
+    uint32_t _next_seq_num;
+    uint16_t _latest_block_len;
+    bool _logging_started;
+    uint32_t _last_response_time;
+    uint32_t _last_send_time;
+    uint8_t _next_block_number_to_resend;
+    bool _sending_to_client;
+
+    void Log_Write_DF_MAV(DataFlash_MAVLink &df);
+    
+    void internal_error();
+    uint16_t bufferspace_available() override; // in bytes
+    uint8_t remaining_space_in_current_block();
+    // write buffer
+    uint8_t _blockcount_free;
+    uint8_t _blockcount;
+    struct dm_block *_blocks;
+    struct dm_block *_current_block;
+    struct dm_block *next_block();
+
+    void periodic_10Hz(uint32_t now);
+    void periodic_1Hz(uint32_t now);
+    void periodic_fullrate(uint32_t now);
+    
+    void stats_init();
+    void stats_reset();
+    void stats_collect();
+    void stats_log();
+    uint32_t _stats_last_collected_time;
+    uint32_t _stats_last_logged_time;
+    uint8_t mavlink_seq;
+
+    /* we currently ignore requests to start a new log.  Notionally we
+     * could close the currently logging session and hope the client
+     * re-opens one */
+    uint16_t start_new_log(void) override {
+        return 0;
+    }
+    bool ReadBlock(void *pkt, uint16_t size) override {
+        return false;
+    }
+    // performance counters
+    AP_HAL::Util::perf_counter_t  _perf_errors;
+    AP_HAL::Util::perf_counter_t  _perf_packing;
+    AP_HAL::Util::perf_counter_t  _perf_overruns;
+};
+
+#endif // DATAFLASH_MAVLINK_SUPPORT
+
+#endif // DATAFLASH_MAVLINK_H
+

libraries/DataFlash/LogFile.cpp

@@ -15,6 +15,7 @@
 #include "DataFlash_SITL.h"
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"
+#include "DataFlash_MAVLink.h"
 #include "DFMessageWriter.h"
 
 extern const AP_HAL::HAL& hal;
@@ -22,7 +23,7 @@ extern const AP_HAL::HAL& hal;
 void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_types)
 {
     if (_next_backend == DATAFLASH_MAX_BACKENDS) {
-        hal.scheduler->panic("Too many backends");
+        AP_HAL::panic("Too many backends");
         return;
     }
     _num_types = num_types;
@@ -47,6 +48,27 @@ void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_ty
     }
 #endif
 
+#if DATAFLASH_MAVLINK_SUPPORT
+    if (_params.backend_types == DATAFLASH_BACKEND_MAVLINK ||
+        _params.backend_types == DATAFLASH_BACKEND_BOTH) {
+        if (_next_backend == DATAFLASH_MAX_BACKENDS) {
+            AP_HAL::panic("Too many backends");
+            return;
+        }
+        DFMessageWriter_DFLogStart *message_writer =
+            new DFMessageWriter_DFLogStart(_firmware_string);
+        if (message_writer != NULL)  {
+            backends[_next_backend] = new DataFlash_MAVLink(*this,
+                                                            message_writer);
+        }
+        if (backends[_next_backend] == NULL) {
+            hal.console->printf("Unable to open DataFlash_MAVLink");
+        } else {
+            _next_backend++;
+        }
+    }
+#endif
+
     for (uint8_t i=0; i<_next_backend; i++) {
         backends[i]->Init();
     }

libraries/DataFlash/LogStructure.h

@@ -550,6 +550,28 @@ struct PACKED log_GYRO {
     float GyrX, GyrY, GyrZ;
 };
 
+struct PACKED log_DF_MAV_Stats {
+    LOG_PACKET_HEADER;
+    uint32_t timestamp;
+    uint32_t seqno;
+    uint32_t dropped;
+    uint32_t retries;
+    uint32_t resends;
+    uint8_t internal_errors; // uint8_t - wishful thinking?
+    uint8_t state_free_avg;
+    uint8_t state_free_min;
+    uint8_t state_free_max;
+    uint8_t state_pending_avg;
+    uint8_t state_pending_min;
+    uint8_t state_pending_max;
+    uint8_t state_sent_avg;
+    uint8_t state_sent_min;
+    uint8_t state_sent_max;
+    // uint8_t state_retry_avg;
+    // uint8_t state_retry_min;
+    // uint8_t state_retry_max;
+};
+
 struct PACKED log_ORGN {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -634,7 +656,9 @@ Format characters in the format string for binary log messages
     { LOG_MODE_MSG, sizeof(log_Mode), \
       "MODE", "QMB",         "TimeUS,Mode,ModeNum" }, \
     { LOG_RFND_MSG, sizeof(log_RFND), \
-      "RFND", "QCC",         "TimeUS,Dist1,Dist2" }
+            "RFND", "QCC",         "TimeUS,Dist1,Dist2" }, \
+    { LOG_DF_MAV_STATS, sizeof(log_DF_MAV_Stats), \
+      "DMS", "IIIIIBBBBBBBBBB",         "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }
 
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \
@@ -839,6 +863,7 @@ enum LogMessages {
     LOG_NKF7_MSG,
     LOG_NKF8_MSG,
     LOG_NKF9_MSG,
+    LOG_DF_MAV_STATS,
 };
 
 enum LogOriginType {