DataFlash: auto-add FMT headers to binary logs if needed

this copes with dataflash wrapping

libraries/DataFlash/DataFlash_Block.cpp

@@ -185,16 +185,20 @@ bool DataFlash_Block::NeedErase(void)
     return version != DF_LOGGING_FORMAT;
 }
 
-/*
-  find the number of pages in a log
+/**
+  get raw data from a log
  */
-int16_t DataFlash_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
+int16_t DataFlash_Block::get_log_data_raw(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
 {
     uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
+
     if (offset >= data_page_size) {
         page += offset / data_page_size;
         offset = offset % data_page_size;
-        page = page % df_NumPages;
+        if (page > df_NumPages) {
+            // pages are one based, not zero
+            page = 1 + page - df_NumPages;
+        }
     }
     if (log_write_started || df_Read_PageAdr != page) {
         StartRead(page);
@@ -202,6 +206,47 @@ int16_t DataFlash_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t
 
     df_Read_BufferIdx = offset + sizeof(struct PageHeader);
     ReadBlock(data, len);
+
     return (int16_t)len;
 }
 
+/**
+  get data from a log, accounting for adding FMT headers
+ */
+int16_t DataFlash_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
+{
+    if (offset == 0) {
+        uint8_t header[3];
+        get_log_data_raw(log_num, page, 0, 3, header);
+        adding_fmt_headers = (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2 || header[2] != LOG_FORMAT_MSG);
+    }
+    uint16_t ret = 0;
+
+    if (adding_fmt_headers) {
+        // the log doesn't start with a FMT message, we need to add
+        // them
+        const uint16_t fmt_header_size = _num_types * sizeof(struct log_Format);
+        while (offset < fmt_header_size && len > 0) {
+            struct log_Format pkt;
+            uint8_t t = offset / sizeof(pkt);
+            uint8_t ofs = offset % sizeof(pkt);
+            Log_Fill_Format(&_structures[t], pkt);
+            uint8_t n = sizeof(pkt) - ofs;
+            if (n > len) {
+                n = len;
+            }
+            memcpy(data, ofs + (uint8_t *)&pkt, n);
+            data += n;
+            offset += n;
+            len -= n;
+            ret += n;
+        }
+        offset -= fmt_header_size;
+    }
+
+    if (len > 0) {
+        ret += get_log_data_raw(log_num, page, offset, len, data);
+    }
+
+    return ret;
+}

libraries/DataFlash/DataFlash_Block.h

@@ -56,6 +56,9 @@ private:
     uint16_t df_FilePage;
     bool log_write_started;
 
+    // offset from adding FMT messages to log data
+    bool adding_fmt_headers;
+
     /*
       functions implemented by the board specific backends
      */