AP_Logger: make block logger conform to mavlink expectations of numbering

add support for log time to block logger
refactor rotation into backed. Don't start logs when erasing
correct log start logic
separate read and write points so that requesting log information does not corrupt the current log
when starting a new log stop logging first
clear the write buffer when starting a new log
insert utc time when requesting info for the current log
stop logging and request formats again when starting a new log
cope with erase happening while we are logging
keep pushing out startup messages even when format messages are done
don't log to the gcs in the io thread
don't start new logs in the io thread
don't validate logs while erasing
flush logs when stopping logging
account for page header when calculating logs sizes
don't return data when asked for more data than in the log
optimize locking and use separate semaphore to mediate ring buffer access
stop logging when the chip is full and send a notification
calculate logs sizes correctly even when they wrap
read log data correctly even when it wraps
add stats support to block logger
reset dropped when starting a new log
fail logging when the chip is full
refactor critical bufferspace checks
increase messagewriter budget to 250us and to 300us for FMT

libraries/AP_Logger/AP_Logger.cpp

@@ -54,8 +54,8 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = {
     AP_GROUPINFO("_BACKEND_TYPE",  0, AP_Logger, _params.backend_types,       uint8_t(HAL_LOGGING_BACKENDS_DEFAULT)),
 
     // @Param: _FILE_BUFSIZE
-    // @DisplayName: Maximum AP_Logger File Backend buffer size (in kilobytes)
+    // @DisplayName: Maximum AP_Logger File and Block Backend buffer size (in kilobytes)
-    // @Description: The AP_Logger_File backend uses a buffer to store data before writing to the block device.  Raising this value may reduce "gaps" in your SD card logging.  This buffer size may be reduced depending on available memory.  PixHawk requires at least 4 kilobytes.  Maximum value available here is 64 kilobytes.
+    // @Description: The File and Block backends use a buffer to store data before writing to the block device.  Raising this value may reduce "gaps" in your SD card logging.  This buffer size may be reduced depending on available memory.  PixHawk requires at least 4 kilobytes.  Maximum value available here is 64 kilobytes.
     // @User: Standard
     AP_GROUPINFO("_FILE_BUFSIZE",  1, AP_Logger, _params.file_bufsize,       HAL_LOGGING_FILE_BUFSIZE),
 
@@ -75,7 +75,7 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = {
 
     // @Param: _FILE_DSRMROT
     // @DisplayName: Stop logging to current file on disarm
-    // @Description: When set, the current log file is closed when the vehicle is disarmed.  If LOG_DISARMED is set then a fresh log will be opened.
+    // @Description: When set, the current log file is closed when the vehicle is disarmed.  If LOG_DISARMED is set then a fresh log will be opened. Applies to the File and Block logging backends.
     // @Values: 0:Disabled,1:Enabled
     // @User: Standard
     AP_GROUPINFO("_FILE_DSRMROT",  4, AP_Logger, _params.file_disarm_rot,       0),

libraries/AP_Logger/AP_Logger_Backend.cpp

@@ -51,11 +51,20 @@ AP_Logger_Backend::vehicle_startup_message_Writer AP_Logger_Backend::vehicle_mes
 void AP_Logger_Backend::periodic_10Hz(const uint32_t now)
 {
 }
+
 void AP_Logger_Backend::periodic_1Hz()
 {
+    if (_rotate_pending && !logging_enabled()) {
+        _rotate_pending = false;
+        // handle log rotation once we stop logging
+        stop_logging();
     }
+    df_stats_log();
+}
+
 void AP_Logger_Backend::periodic_fullrate()
 {
+    push_log_blocks();
 }
 
 void AP_Logger_Backend::periodic_tasks()
@@ -74,8 +83,10 @@ void AP_Logger_Backend::periodic_tasks()
 
 void AP_Logger_Backend::start_new_log_reset_variables()
 {
+    _dropped = 0;
     _startup_messagewriter->reset();
     _front.backend_starting_new_log(this);
+    _log_file_size_bytes = 0;
 }
 
 // this method can be overridden to do extra things with your buffer.
@@ -410,6 +421,18 @@ bool AP_Logger_Backend::ShouldLog(bool is_critical)
     return true;
 }
 
+void AP_Logger_Backend::PrepForArming()
+{
+    if (_rotate_pending) {
+        _rotate_pending = false;
+        stop_logging();
+    }
+    if (logging_started()) {
+        return;
+    }
+    start_new_log();
+}
+
 bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...)
 {
     char msg[65] {}; // sizeof(log_Message.msg) + null-termination
@@ -445,3 +468,100 @@ bool AP_Logger_Backend::Write_Rally()
     // kick off asynchronous write:
     return _startup_messagewriter->writeallrallypoints();
 }
+
+/*
+  convert a list entry number back into a log number (which can then
+  be converted into a filename).  A "list entry number" is a sequence
+  where the oldest log has a number of 1, the second-from-oldest 2,
+  and so on.  Thus the highest list entry number is equal to the
+  number of logs.
+*/
+uint16_t AP_Logger_Backend::log_num_from_list_entry(const uint16_t list_entry)
+{
+    uint16_t oldest_log = find_oldest_log();
+    if (oldest_log == 0) {
+        return 0;
+    }
+
+    uint32_t log_num = oldest_log + list_entry - 1;
+    if (log_num > MAX_LOG_FILES) {
+        log_num -= MAX_LOG_FILES;
+    }
+    return (uint16_t)log_num;
+}
+
+// find_oldest_log - find oldest log
+// returns 0 if no log was found
+uint16_t AP_Logger_Backend::find_oldest_log()
+{
+    if (_cached_oldest_log != 0) {
+        return _cached_oldest_log;
+    }
+
+    uint16_t last_log_num = find_last_log();
+    if (last_log_num == 0) {
+        return 0;
+    }
+
+    _cached_oldest_log = last_log_num - get_num_logs() + 1;
+
+    return _cached_oldest_log;
+}
+
+void AP_Logger_Backend::vehicle_was_disarmed()
+{
+    if (_front._params.file_disarm_rot) {
+        // rotate our log.  Closing the current one and letting the
+        // logging restart naturally based on log_disarmed should do
+        // the trick:
+        _rotate_pending = true;
+    }
+}
+
+// this sensor is enabled if we should be logging at the moment
+bool AP_Logger_Backend::logging_enabled() const
+{
+    if (hal.util->get_soft_armed() ||
+        _front.log_while_disarmed()) {
+        return true;
+    }
+    return false;
+}
+
+void AP_Logger_Backend::Write_AP_Logger_Stats_File(const struct df_stats &_stats)
+{
+    struct log_DSF pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_DF_FILE_STATS),
+        time_us         : AP_HAL::micros64(),
+        dropped         : _dropped,
+        blocks          : _stats.blocks,
+        bytes           : _stats.bytes,
+        buf_space_min   : _stats.buf_space_min,
+        buf_space_max   : _stats.buf_space_max,
+        buf_space_avg   : (_stats.blocks) ? (_stats.buf_space_sigma / _stats.blocks) : 0,
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
+void AP_Logger_Backend::df_stats_gather(const uint16_t bytes_written, uint32_t space_remaining) {
+    if (space_remaining < stats.buf_space_min) {
+        stats.buf_space_min = space_remaining;
+    }
+    if (space_remaining > stats.buf_space_max) {
+        stats.buf_space_max = space_remaining;
+    }
+    stats.buf_space_sigma += space_remaining;
+    stats.bytes += bytes_written;
+    _log_file_size_bytes += bytes_written;
+    stats.blocks++;
+}
+
+void AP_Logger_Backend::df_stats_clear() {
+    memset(&stats, '\0', sizeof(stats));
+    stats.buf_space_min = -1;
+}
+
+void AP_Logger_Backend::df_stats_log() {
+    Write_AP_Logger_Stats_File(stats);
+    df_stats_clear();
+}

libraries/AP_Logger/AP_Logger_Backend.h

@@ -4,6 +4,8 @@
 
 class LoggerMessageWriter_DFLogStart;
 
+#define MAX_LOG_FILES 500
+
 class AP_Logger_Backend
 {
 
@@ -20,7 +22,6 @@ public:
     // erase handling
     virtual void EraseAll() = 0;
 
-    virtual bool NeedPrep() = 0;
     virtual void Prep() = 0;
 
     /* Write a block of data at current offset */
@@ -34,22 +35,23 @@ public:
 
     bool WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical);
 
-    // high level interface
+    // high level interface, indexed by the position in the list of logs
     virtual uint16_t find_last_log() = 0;
-    virtual void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) = 0;
+    virtual void get_log_boundaries(uint16_t list_entry, uint32_t & start_page, uint32_t & end_page) = 0;
-    virtual void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) = 0;
+    virtual void get_log_info(uint16_t list_entry, uint32_t &size, uint32_t &time_utc) = 0;
-    virtual int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) = 0;
+    virtual int16_t get_log_data(uint16_t list_entry, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) = 0;
     virtual uint16_t get_num_logs() = 0;
+    virtual uint16_t find_oldest_log();
 
     virtual bool logging_started(void) const = 0;
 
-    virtual void Init() { }
+    virtual void Init() = 0;
 
     virtual uint32_t bufferspace_available() = 0;
 
-    virtual void PrepForArming() { }
+    virtual void PrepForArming();
 
-    virtual void start_new_log() { };
+    virtual void start_new_log() { }
 
     /* stop logging - close output files etc etc.
      *
@@ -115,16 +117,15 @@ public:
     bool Write(uint8_t msg_type, va_list arg_list, bool is_critical=false);
 
     // these methods are used when reporting system status over mavlink
-    virtual bool logging_enabled() const = 0;
+    virtual bool logging_enabled() const;
     virtual bool logging_failed() const = 0;
 
-    virtual void vehicle_was_disarmed() { };
+    virtual void vehicle_was_disarmed();
 
     bool Write_Unit(const struct UnitStructure *s);
     bool Write_Multiplier(const struct MultiplierStructure *s);
     bool Write_Format_Units(const struct LogStructure *structure);
 
-
 protected:
 
     AP_Logger &_front;
@@ -147,20 +148,62 @@ protected:
     LoggerMessageWriter_DFLogStart *_startup_messagewriter;
     bool _writing_startup_messages;
 
+    uint16_t _cached_oldest_log;
+
     uint32_t _dropped;
+    uint32_t _log_file_size_bytes;
+    // should we rotate when we next stop logging
+    bool _rotate_pending;
 
     // must be called when a new log is being started:
     virtual void start_new_log_reset_variables();
+    // convert between log numbering in storage and normalized numbering
+    uint16_t log_num_from_list_entry(const uint16_t list_entry);
+
+    uint32_t critical_message_reserved_space(uint32_t bufsize) const {
+        // possibly make this a proportional to buffer size?
+        uint32_t ret = 1024;
+        if (ret > bufsize) {
+            // in this case you will only get critical messages
+            ret = bufsize;
+        }
+        return ret;
+    };
+    uint32_t non_messagewriter_message_reserved_space(uint32_t bufsize) const {
+        // possibly make this a proportional to buffer size?
+        uint32_t ret = 1024;
+        if (ret >= bufsize) {
+            // need to allow messages out from the messagewriters.  In
+            // this case while you have a messagewriter you won't get
+            // any other messages.  This should be a corner case!
+            ret = 0;
+        }
+        return ret;
+    };
 
     virtual bool _WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) = 0;
 
     bool _initialised;
 
+    void df_stats_gather(uint16_t bytes_written, uint32_t space_remaining);
+    void df_stats_log();
+    void df_stats_clear();
+
 private:
+    // statistics support
+    struct df_stats {
+        uint16_t blocks;
+        uint32_t bytes;
+        uint32_t buf_space_min;
+        uint32_t buf_space_max;
+        uint32_t buf_space_sigma;
+    };
+    struct df_stats stats;
 
     uint32_t _last_periodic_1Hz;
     uint32_t _last_periodic_10Hz;
     bool have_logged_armed;
 
+    void Write_AP_Logger_Stats_File(const struct df_stats &_stats);
     void validate_WritePrioritisedBlock(const void *pBuffer, uint16_t size);
 };

libraries/AP_Logger/AP_Logger_Block.cpp

@@ -6,22 +6,25 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <stdio.h>
+#include <AP_RTC/AP_RTC.h>
 #include <GCS_MAVLink/GCS.h>
 
 const extern AP_HAL::HAL& hal;
 
 // the last page holds the log format in first 4 bytes. Please change
 // this if (and only if!) the low level format changes
-#define DF_LOGGING_FORMAT    0x1901201A
+#define DF_LOGGING_FORMAT    0x1901201B
 
 AP_Logger_Block::AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
     writebuf(0),
     AP_Logger_Backend(front, writer)
 {
+    // buffer is used for both reads and writes so access must always be within the semaphore
     buffer = (uint8_t *)hal.util->malloc_type(page_size_max, AP_HAL::Util::MEM_DMA_SAFE);
     if (buffer == nullptr) {
         AP_HAL::panic("Out of DMA memory for logging");
     }
+    df_stats_clear();
 }
 
 // init is called after backend init
@@ -56,8 +59,6 @@ void AP_Logger_Block::Init(void)
     WITH_SEMAPHORE(sem);
 
     hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Logger_Block::io_timer, void));
-
-    AP_Logger_Backend::Init();
 }
 
 uint32_t AP_Logger_Block::bufferspace_available()
@@ -71,7 +72,6 @@ uint32_t AP_Logger_Block::bufferspace_available()
 void AP_Logger_Block::StartWrite(uint32_t PageAdr)
 {
     df_PageAdr    = PageAdr;
-    log_write_started = true;
 }
 
 void AP_Logger_Block::FinishWrite(void)
@@ -87,13 +87,25 @@ void AP_Logger_Block::FinishWrite(void)
 
     // when starting a new sector, erase it
     if ((df_PageAdr-1) % df_PagePerBlock == 0) {
+        // if we have wrapped over an existing log, force the oldest to be recalculated
+        if (_cached_oldest_log > 0) {
+            uint16_t log_num = StartRead(df_PageAdr);
+            if (log_num != 0xFFFF && log_num >= _cached_oldest_log) {
+                _cached_oldest_log = 0;
+            }
+        }
+        // are we about to erase a sector with our own headers in it?
+        if (df_Write_FilePage > df_NumPages - df_PagePerBlock) {
+            chip_full = true;
+            return;
+        }
         SectorErase(df_PageAdr / df_PagePerBlock);
     }
 }
 
 bool AP_Logger_Block::WritesOK() const
 {
-    if (!CardInserted()) {
+    if (!CardInserted() || erase_started) {
         return false;
     }
     return true;
@@ -109,20 +121,56 @@ bool AP_Logger_Block::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
     }
 
     if (!WriteBlockCheckStartupMessages()) {
+        _dropped++;
         return false;
     }
 
-    if (writebuf.space() < size) {
+    if (!write_sem.take(1)) {
-        // no room in buffer
+        return false;
+    }
+
+    uint32_t space = writebuf.space();
+
+    if (_writing_startup_messages &&
+        _startup_messagewriter->fmt_done()) {
+        // the state machine has called us, and it has finished
+        // writing format messages out.  It can always get back to us
+        // with more messages later, so let's leave room for other
+        // things:
+        const uint32_t now = AP_HAL::millis();
+        const bool must_dribble = (now - last_messagewrite_message_sent) > 100;
+        if (!must_dribble &&
+            space < non_messagewriter_message_reserved_space(writebuf.get_size())) {
+            // this message isn't dropped, it will be sent again...
+            write_sem.give();
+            return false;
+        }
+        last_messagewrite_message_sent = now;
+    } else {
+        // we reserve some amount of space for critical messages:
+        if (!is_critical && space < critical_message_reserved_space(writebuf.get_size())) {
+            _dropped++;
+            write_sem.give();
+            return false;
+        }
+    }
+
+    // if no room for entire message - drop it:
+    if (space < size) {
+        _dropped++;
+        write_sem.give();
         return false;
     }
 
     writebuf.write((uint8_t*)pBuffer, size);
+    df_stats_gather(size, writebuf.space());
+    write_sem.give();
+
     return true;
 }
 
-
+// read from the page address and return the file number at that location
-void AP_Logger_Block::StartRead(uint32_t PageAdr)
+uint16_t AP_Logger_Block::StartRead(uint32_t PageAdr)
 {
     df_Read_PageAdr   = PageAdr;
 
@@ -132,13 +180,32 @@ void AP_Logger_Block::StartRead(uint32_t PageAdr)
     } else {
         PageToBuffer(df_Read_PageAdr);
     }
+    return ReadHeaders();
+}
 
+// read the headers at the current read point returning the file number
+uint16_t AP_Logger_Block::ReadHeaders()
+{
     // We are starting a new page - read FileNumber and FilePage
     struct PageHeader ph;
     BlockRead(0, &ph, sizeof(ph));
     df_FileNumber = ph.FileNumber;
     df_FilePage   = ph.FilePage;
+#if BLOCK_LOG_VALIDATE
+    if (ph.crc != DF_LOGGING_FORMAT + df_FilePage && df_FileNumber != 0xFFFF) {
+        printf("ReadHeaders: invalid block read at %d\n", df_Read_PageAdr);
+    }
+#endif
     df_Read_BufferIdx = sizeof(ph);
+    // we are at the start of a file, read the file header
+    if (df_FilePage == 1) {
+        struct FileHeader fh;
+        BlockRead(0, &fh, sizeof(fh));
+        df_FileTime = fh.utc_secs;
+        df_Read_BufferIdx += sizeof(fh);
+    }
+
+    return df_FileNumber;
 }
 
 bool AP_Logger_Block::ReadBlock(void *pBuffer, uint16_t size)
@@ -172,23 +239,19 @@ bool AP_Logger_Block::ReadBlock(void *pBuffer, uint16_t size)
             }
 
             // We are starting a new page - read FileNumber and FilePage
-            struct PageHeader ph;
+            ReadHeaders();
-            if (!BlockRead(0, &ph, sizeof(ph))) {
-                return false;
-            }
-            df_FileNumber = ph.FileNumber;
-            df_FilePage   = ph.FilePage;
-
-            df_Read_BufferIdx = sizeof(ph);
         }
     }
     return true;
 }
 
-void AP_Logger_Block::SetFileNumber(uint16_t FileNumber)
+// initialize the log data for the given file number
+void AP_Logger_Block::StartLogFile(uint16_t FileNumber)
 {
     df_FileNumber = FileNumber;
+    df_Write_FileNumber = FileNumber;
     df_FilePage = 1;
+    df_Write_FilePage = 1;
 }
 
 uint16_t AP_Logger_Block::GetFileNumber()
@@ -198,39 +261,105 @@ uint16_t AP_Logger_Block::GetFileNumber()
 
 void AP_Logger_Block::EraseAll()
 {
-    WITH_SEMAPHORE(sem);
-    if (erase_started) {
-        // already erasing
-        return;
-    }
-
-    gcs().send_text(MAV_SEVERITY_INFO, "Chip erase started");
-    // reset the format version and wrapped status so that any incomplete erase will be caught
-    Sector4kErase(get_sector(df_NumPages));
-
-    log_write_started = false;
-
-    StartErase();
-    erase_started = true;
-}
-
-bool AP_Logger_Block::NeedPrep(void)
-{
-    return NeedErase();
-}
-
-void AP_Logger_Block::Prep()
-{
-    WITH_SEMAPHORE(sem);
     if (hal.util->get_soft_armed()) {
         // do not want to do any filesystem operations while we are e.g. flying
         return;
     }
+
+    // push out the message before stopping logging
+    if (!erase_started) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Chip erase started");
+    }
+
+    WITH_SEMAPHORE(sem);
+
+    if (erase_started) {
+        // already erasing
+        return;
+    }
+    erase_started = true;
+
+    // remember what we were doing
+    new_log_pending = log_write_started;
+
+    // throw away everything
+    log_write_started = false;
+    writebuf.clear();
+
+    // reset the format version and wrapped status so that any incomplete erase will be caught
+    Sector4kErase(get_sector(df_NumPages));
+
+    StartErase();
+}
+
+void AP_Logger_Block::periodic_1Hz()
+{
+    AP_Logger_Backend::periodic_1Hz();
+
+    if (!io_thread_alive()) {
+        if (warning_decimation_counter == 0 && _initialised) {
+            // we don't print this error unless we did initialise. When _initialised is set to true
+            // we register the IO timer callback
+            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AP_Logger: IO thread died");
+        }
+        if (warning_decimation_counter++ > 57) {
+            warning_decimation_counter = 0;
+        }
+        _initialised = false;
+    } else if (chip_full) {
+        if (warning_decimation_counter == 0) {
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Chip full, logging stopped");
+        }
+        if (warning_decimation_counter++ > 57) {
+            warning_decimation_counter = 0;
+        }
+    }
+}
+
+// EraseAll is asynchronous, but we must not start a new
+// log in a child thread so this task picks up the hint from the io timer
+// keeping locking to a minimum
+void AP_Logger_Block::periodic_10Hz(const uint32_t now)
+{
+    if (erase_started || InErase()) {
+        return;
+    }
+
+    // don't print status messages in io thread, do it here
+    switch (status_msg) {
+    case StatusMessage::ERASE_COMPLETE:
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Chip erase complete");
+        status_msg = StatusMessage::NONE;
+        break;
+    case StatusMessage::RECOVERY_COMPLETE:
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Log recovery complete");
+        status_msg = StatusMessage::NONE;
+        break;
+    case StatusMessage::NONE:
+        break;
+    }
+
+    // EraseAll should only set this in the main thread
+    if (new_log_pending) {
+        start_new_log();
+    }
+}
+
+void AP_Logger_Block::Prep()
+{
+    if (hal.util->get_soft_armed()) {
+        // do not want to do any filesystem operations while we are e.g. flying
+        return;
+    }
+
+    WITH_SEMAPHORE(sem);
+
     if (NeedErase()) {
         EraseAll();
-    }
+    } else {
         validate_log_structure();
     }
+}
 
 /*
  *  we need to erase if the logging format has changed
@@ -238,11 +367,11 @@ void AP_Logger_Block::Prep()
 bool AP_Logger_Block::NeedErase(void)
 {
     uint32_t version = 0;
-    StartRead(df_NumPages+1); // last page
+    PageToBuffer(df_NumPages+1); // last page
-
     BlockRead(0, &version, sizeof(version));
-    StartRead(1);
     if (version == DF_LOGGING_FORMAT) {
+        // only leave the read point in a sane place if we are not about to destroy everything
+        StartRead(1);
         return false;
     }
     return true;
@@ -254,12 +383,11 @@ bool AP_Logger_Block::NeedErase(void)
 void AP_Logger_Block::validate_log_structure()
 {
     WITH_SEMAPHORE(sem);
-    bool wrapped = check_wrapped();
+    bool wrapped = is_wrapped();
     uint32_t page = 1;
     uint32_t page_start = 1;
 
-    StartRead(page);
+    uint16_t file = StartRead(page);
-    uint16_t file = GetFileNumber();
     uint16_t first_file = file;
     uint16_t next_file = file;
     uint16_t last_file = 0;
@@ -270,13 +398,11 @@ void AP_Logger_Block::validate_log_structure()
             break;
         }
         page = end_page + 1;
-        StartRead(page);
+        file = StartRead(page);
-        file = GetFileNumber();
         next_file++;
-        // skip over the rest of an erased blcok
+        // skip over the rest of an erased block
         if (wrapped && file == 0xFFFF) {
-            StartRead((get_block(page) + 1) * df_PagePerBlock + 1);
+            file = StartRead((get_block(page) + 1) * df_PagePerBlock + 1);
-            file = GetFileNumber();
         }
         if (wrapped && file < next_file) {
             page_start = page;
@@ -291,34 +417,40 @@ void AP_Logger_Block::validate_log_structure()
     }
 
     if (file != 0xFFFF && file != next_file && page <= df_NumPages && page > 0) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Found corrupt log %d at 0x%04X, erasing", int(file), unsigned(page));
+        hal.console->printf("Found corrupt log %d at 0x%04X, erasing", int(file), unsigned(page));
         df_EraseFrom = page;
     } else if (next_file != 0xFFFF && page > 0 && next_file > 1) { // chip is empty
-        gcs().send_text(MAV_SEVERITY_INFO, "Found %d complete logs at 0x%04X-0x%04X", int(next_file - first_file), unsigned(page_start), unsigned(page - 1));
+        hal.console->printf("Found %d complete logs at 0x%04X-0x%04X", int(next_file - first_file), unsigned(page_start), unsigned(page - 1));
     }
 }
 
 /**
-  get raw data from a log
+ * get raw data from a log - page is the start page of the log, offset is the offset within the log starting at that page
  */
 int16_t AP_Logger_Block::get_log_data_raw(uint16_t log_num, uint32_t page, uint32_t offset, uint16_t len, uint8_t *data)
 {
     WITH_SEMAPHORE(sem);
-    uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
+    const uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
+    const uint16_t first_page_size = data_page_size - sizeof(struct FileHeader);
+
+    // offset is the true offset in the file, so we have to calculate the offset accounting for page headers
+    if (offset >= first_page_size) {
+        offset -= first_page_size;
+        page = page + offset / data_page_size + 1;
+        offset %= data_page_size;
 
-    if (offset >= data_page_size) {
-        page += offset / data_page_size;
-        offset = offset % data_page_size;
         if (page > df_NumPages) {
-            // pages are one based, not zero
+            page = page % df_NumPages;
-            page = 1 + page - df_NumPages;
         }
     }
-    if (log_write_started || df_Read_PageAdr != page) {
-        StartRead(page);
-    }
 
-    df_Read_BufferIdx = offset + sizeof(struct PageHeader);
+    // Sanity check we haven't been asked for an offset beyond the end of the log
+    if (StartRead(page) != log_num) {
+        return -1;
+    }
+
+    df_Read_BufferIdx += offset;
+
     if (!ReadBlock(data, len)) {
         return -1;
     }
@@ -329,44 +461,23 @@ int16_t AP_Logger_Block::get_log_data_raw(uint16_t log_num, uint32_t page, uint3
 /**
   get data from a log, accounting for adding FMT headers
  */
-int16_t AP_Logger_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
+int16_t AP_Logger_Block::get_log_data(uint16_t list_entry, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
 {
-    WITH_SEMAPHORE(sem);
+    const uint16_t log_num = log_num_from_list_entry(list_entry);
-    if (offset == 0) {
+
-        uint8_t header[3];
+    if (log_num == 0) {
-        if (get_log_data_raw(log_num, page, 0, 3, header) == -1) {
+        // that failed - probably no logs
         return -1;
     }
-        adding_fmt_headers = (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2 || header[2] != LOG_FORMAT_MSG);
+
-    }
+    //printf("get_log_data(%d, %d, %d, %d)\n", log_num, page, offset, len);
+    WITH_SEMAPHORE(sem);
+
     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);
-            Fill_Format(structure(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) {
         const int16_t bytes = get_log_data_raw(log_num, page, offset, len, data);
         if (bytes == -1) {
-            return ret == 0 ? -1 : ret;
+            return -1;
         }
         ret += bytes;
     }
@@ -375,8 +486,8 @@ int16_t AP_Logger_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t
 }
 
 
-// This function determines the number of whole or partial log files in the AP_Logger
+// This function determines the number of whole log files in the AP_Logger
-// Wholly overwritten files are (of course) lost.
+// partial logs are rejected as without the headers they are relatively useless
 uint16_t AP_Logger_Block::get_num_logs(void)
 {
     WITH_SEMAPHORE(sem);
@@ -387,22 +498,24 @@ uint16_t AP_Logger_Block::get_num_logs(void)
         return 0;
     }
 
-    StartRead(1);
+    uint32_t first = StartRead(1);
-    uint32_t first = GetFileNumber();
     
     if (first == 0xFFFF) {
         return 0;
     }
 
     lastpage = find_last_page();
-    StartRead(lastpage);
+    last = StartRead(lastpage);
-    last = GetFileNumber();
+
-    if (check_wrapped()) {
+    if (is_wrapped()) {
         // if we wrapped then the rest of the block will be filled with 0xFFFF because we always erase
         // a block before writing to it, in order to find the first page we therefore have to read after the
         // next block boundary
-        StartRead((get_block(lastpage) + 1) * df_PagePerBlock + 1);
+        first = StartRead((get_block(lastpage) + 1) * df_PagePerBlock + 1);
-        first = GetFileNumber();
+        // unless we happen to land on the first page of the file that is being overwritten we skip to the next file
+        if (df_FilePage > 1) {
+            first++;
+        }
     }
 
     if (last == first) {
@@ -412,69 +525,112 @@ uint16_t AP_Logger_Block::get_num_logs(void)
     return (last - first + 1);
 }
 
-
+// stop logging and flush any remaining data
-// This function starts a new log file in the AP_Logger
+void AP_Logger_Block::stop_logging(void)
-void AP_Logger_Block::start_new_log(void)
 {
     WITH_SEMAPHORE(sem);
+
+    log_write_started = false;
+
+    // complete writing any previous log
+    while (writebuf.available()) {
+        write_log_page();
+    }
+    writebuf.clear();
+}
+
+// This function starts a new log file in the AP_Logger
+// no actual data should be written to the storage here
+// that should all be handled by the IO thread
+void AP_Logger_Block::start_new_log(void)
+{
+    if (erase_started) {
+        // already erasing
+        return;
+    }
+
+    WITH_SEMAPHORE(sem);
+
+    if (logging_started()) {
+        stop_logging();
+    }
+
+    // no need to schedule this anymore
+    new_log_pending = false;
+
     uint32_t last_page = find_last_page();
 
     StartRead(last_page);
 
+    log_write_started = true;
+    uint16_t new_log_num = 1;
+
     if (find_last_log() == 0 || GetFileNumber() == 0xFFFF) {
-        SetFileNumber(1);
+        StartLogFile(new_log_num);
         StartWrite(1);
-        return;
-    }
-
-    uint16_t new_log_num;
-
     // Check for log of length 1 page and suppress
-    if (df_FilePage <= 1) {
+    } else if (df_FilePage <= 1) {
         new_log_num = GetFileNumber();
         // Last log too short, reuse its number
         // and overwrite it
-        SetFileNumber(new_log_num);
+        StartLogFile(new_log_num);
         StartWrite(last_page);
     } else {
         new_log_num = GetFileNumber()+1;
         if (last_page == 0xFFFF) {
             last_page=0;
         }
-        SetFileNumber(new_log_num);
+        StartLogFile(new_log_num);
         StartWrite(last_page + 1);
     }
+
+    // save UTC time in the first 4 bytes so that we can retrieve it later
+    uint64_t utc_usec;
+    FileHeader hdr {};
+    if (AP::rtc().get_utc_usec(utc_usec)) {
+        hdr.utc_secs = utc_usec / 1000000U;
+    }
+    writebuf.write((uint8_t*)&hdr, sizeof(FileHeader));
+
+    start_new_log_reset_variables();
+
     return;
 }
 
 // This function finds the first and last pages of a log file
 // The first page may be greater than the last page if the AP_Logger has been filled and partially overwritten.
-void AP_Logger_Block::get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page)
+void AP_Logger_Block::get_log_boundaries(uint16_t list_entry, uint32_t & start_page, uint32_t & end_page)
 {
+    const uint16_t log_num = log_num_from_list_entry(list_entry);
+    if (log_num == 0) {
+        // that failed - probably no logs
+        start_page = 0;
+        end_page = 0;
+        return;
+    }
+
     WITH_SEMAPHORE(sem);
     uint16_t num = get_num_logs();
     uint32_t look;
 
-    if (num == 1) {
+    end_page = find_last_page_of_log(log_num);
-        StartRead(df_NumPages);
+
-        if (GetFileNumber() == 0xFFFF) {
+    if (num == 1 || log_num == 1) {
-            start_page = 1;
+        if (!is_wrapped()) {
-            end_page = find_last_page_of_log((uint16_t)log_num);
-        } else {
-            end_page = find_last_page_of_log((uint16_t)log_num);
-            start_page = end_page + 1;
-        }
-    } else {
-        if (log_num==1) {
-            StartRead(df_NumPages);
-            if (GetFileNumber() == 0xFFFF) {
             start_page = 1;
         } else {
-                start_page = find_last_page() + 1;
+            StartRead(end_page);
+            start_page = (end_page + df_NumPages - df_FilePage) % df_NumPages + 1;
         }
     } else {
-            if (log_num == find_last_log() - num + 1) {
+        // looking for the first log which might have a gap in front of it
-                start_page = find_last_page() + 1;
+        if (list_entry == 1) {
+            StartRead(end_page);
+            if (end_page > df_FilePage) { // log is not wrapped
+                start_page = end_page - df_FilePage + 1;
+            } else { // log is wrapped
+                start_page = (end_page + df_NumPages - df_FilePage) % df_NumPages + 1;
+            }
         } else {
             look = log_num-1;
             do {
@@ -483,21 +639,21 @@ void AP_Logger_Block::get_log_boundaries(uint16_t log_num, uint32_t & start_page
             } while (start_page <= 0 && look >=1);
         }
     }
-    }
+
     if (start_page == df_NumPages + 1 || start_page == 0) {
         start_page = 1;
     }
-    end_page = find_last_page_of_log(log_num);
+
     if (end_page == 0) {
         end_page = start_page;
     }
 
 }
 
-bool AP_Logger_Block::check_wrapped(void)
+// return true if logging has wrapped around to the beginning of the chip
+bool AP_Logger_Block::is_wrapped(void)
 {
-    StartRead(df_NumPages);
+    return StartRead(df_NumPages) != 0xFFFF;
-    return GetFileNumber() != 0xFFFF;
 }
 
 
@@ -506,8 +662,7 @@ uint16_t AP_Logger_Block::find_last_log(void)
 {
     WITH_SEMAPHORE(sem);
     uint32_t last_page = find_last_page();
-    StartRead(last_page);
+    return StartRead(last_page);
-    return GetFileNumber();
 }
 
 // This function finds the last page of the last file
@@ -567,9 +722,8 @@ uint32_t AP_Logger_Block::find_last_page_of_log(uint16_t log_number)
 
     WITH_SEMAPHORE(sem);
 
-    if (check_wrapped()) {
+    if (is_wrapped()) {
-        StartRead(1);
+        bottom = StartRead(1);
-        bottom = GetFileNumber();
         if (bottom > log_number) {
             bottom = find_last_page();
             top = df_NumPages;
@@ -601,43 +755,45 @@ uint32_t AP_Logger_Block::find_last_page_of_log(uint16_t log_number)
         }
     }
 
-    StartRead(top);
+    if (StartRead(top) == log_number) {
-    if (GetFileNumber() == log_number) {
         return top;
     }
 
-    StartRead(bottom);
+    if (StartRead(bottom) == log_number) {
-    if (GetFileNumber() == log_number) {
         return bottom;
     }
 
-    gcs().send_text(MAV_SEVERITY_ERROR, "No last page of log %d at top=%X or bot=%X", int(log_number), unsigned(top), unsigned(bottom));
+    GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "No last page of log %d at top=%X or bot=%X", int(log_number), unsigned(top), unsigned(bottom));
     return 0;
 }
 
-
+void AP_Logger_Block::get_log_info(uint16_t list_entry, uint32_t &size, uint32_t &time_utc)
-void AP_Logger_Block::get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc)
 {
     uint32_t start, end;
 
     WITH_SEMAPHORE(sem);
 
-    get_log_boundaries(log_num, start, end);
+    get_log_boundaries(list_entry, start, end);
     if (end >= start) {
-        size = (end + 1 - start) * (uint32_t)df_PageSize;
+        size = (end + 1 - start) * (uint32_t)(df_PageSize - sizeof(PageHeader));
     } else {
-        size = (df_NumPages + end - start) * (uint32_t)df_PageSize;
+        size = (df_NumPages + end + 1 - start) * (uint32_t)(df_PageSize - sizeof(PageHeader));
-    }
-    time_utc = 0;
     }
 
+    size -= sizeof(FileHeader);
 
-void AP_Logger_Block::PrepForArming()
+    //printf("LOG %d(%d), %d-%d, size %d\n", log_num_from_list_entry(list_entry), list_entry, start, end, size);
-{
+
-    if (logging_started()) {
+    StartRead(start);
-        return;
+
+    // the log we are currently writing
+    if (df_FileTime == 0 && df_FileNumber == df_Write_FileNumber) {
+        uint64_t utc_usec;
+        if (AP::rtc().get_utc_usec(utc_usec)) {
+            df_FileTime = utc_usec / 1000000U;
         }
-    start_new_log();
+    }
+    time_utc = df_FileTime;
 }
 
 // read size bytes of data from the buffer
@@ -647,18 +803,49 @@ bool AP_Logger_Block::BlockRead(uint16_t IntPageAdr, void *pBuffer, uint16_t siz
     return true;
 }
 
+bool AP_Logger_Block::logging_failed() const
+{
+    if (!_initialised) {
+        return true;
+    }
+    if (!io_thread_alive()) {
+        return true;
+    }
+    if (chip_full) {
+        return true;
+    }
+
+    return false;
+}
+
+bool AP_Logger_Block::io_thread_alive() const
+{
+    // if the io thread hasn't had a heartbeat in 5s it is dead
+    // the timeout is longer than might be otherwise required to allow for the FFT running
+    // at the same priority
+    return (AP_HAL::millis() - io_timer_heartbeat) < 5000U;
+}
+
 /*
   IO timer running on IO thread
+  The IO timer runs every 1ms or at 1Khz. The standard flash chip can write rougly 130Kb/s
+  so there is little point in trying to write more than 130 bytes - or 1 page (256 bytes).
+  The W25Q128FV datasheet gives tpp as typically 0.7ms yielding an absolute maximum rate of
+  365Kb/s or just over a page per cycle.
  */
 void AP_Logger_Block::io_timer(void)
 {
-    if (!_initialised) {
+    uint32_t tnow = AP_HAL::millis();
+    io_timer_heartbeat = tnow;
+
+    // don't write anything for the first 2s to give the dataflash chip a chance to be ready
+    if (!_initialised || tnow < 2000) {
         return;
     }
 
+    if (erase_started) {
         WITH_SEMAPHORE(sem);
 
-    if (erase_started) {
         if (InErase()) {
             return;
         }
@@ -669,11 +856,14 @@ void AP_Logger_Block::io_timer(void)
         memcpy(buffer, &version, sizeof(version));
         FinishWrite();
         erase_started = false;
-        gcs().send_text(MAV_SEVERITY_INFO, "Chip erase complete");
+        chip_full = false;
+        status_msg = StatusMessage::ERASE_COMPLETE;
         return;
     }
 
     if (df_EraseFrom > 0) {
+        WITH_SEMAPHORE(sem);
+
         const uint32_t sectors = df_NumPages / df_PagePerSector;
         const uint32_t sectors_in_64k = 0x10000 / (df_PagePerSector * df_PageSize);
         uint32_t next_sector = get_sector(df_EraseFrom);
@@ -688,21 +878,38 @@ void AP_Logger_Block::io_timer(void)
             SectorErase(next_sector / sectors_in_64k);
             next_sector += sectors_in_64k;
         }
-        gcs().send_text(MAV_SEVERITY_WARNING, "Log recovery complete, erased %d blocks", unsigned(blocks_erased));
+        status_msg = StatusMessage::RECOVERY_COMPLETE;
         df_EraseFrom = 0;
     }
 
-    if (!CardInserted() || !log_write_started) {
+    if (!CardInserted() || new_log_pending || chip_full) {
         return;
     }
 
-    while (writebuf.available() >= df_PageSize - sizeof(struct PageHeader)) {
+    // write at most one page
+    if (writebuf.available() >= df_PageSize - sizeof(struct PageHeader)) {
+        WITH_SEMAPHORE(sem);
+
+        write_log_page();
+    }
+}
+
+// write out a page of log data
+void AP_Logger_Block::write_log_page()
+{
     struct PageHeader ph;
-        ph.FileNumber = df_FileNumber;
+    ph.FileNumber = df_Write_FileNumber;
-        ph.FilePage = df_FilePage;
+    ph.FilePage = df_Write_FilePage;
+#if BLOCK_LOG_VALIDATE
+    ph.crc = DF_LOGGING_FORMAT + df_Write_FilePage;
+#endif
     memcpy(buffer, &ph, sizeof(ph));
-        writebuf.read(&buffer[sizeof(ph)], df_PageSize - sizeof(ph));
+    const uint32_t pagesize = df_PageSize - sizeof(ph);
+    uint32_t nbytes = writebuf.read(&buffer[sizeof(ph)], pagesize);
+    if (nbytes <  pagesize) {
+        memset(&buffer[sizeof(ph) + nbytes], 0, pagesize - nbytes);
+    }
     FinishWrite();
-        df_FilePage++;
+    df_Write_FilePage++;
-    }
 }
+

libraries/AP_Logger/AP_Logger_Block.h

@@ -5,6 +5,8 @@
 
 #include "AP_Logger_Backend.h"
 
+#define BLOCK_LOG_VALIDATE 0
+
 class AP_Logger_Block : public AP_Logger_Backend {
 public:
     AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer);
@@ -15,26 +17,50 @@ public:
     // erase handling
     void EraseAll() override;
 
-    bool NeedPrep(void) override;
     void Prep() override;
-    void PrepForArming() override;
 
     // high level interface
     uint16_t find_last_log() override;
-    void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) override;
+    void get_log_boundaries(uint16_t list_entry, uint32_t & start_page, uint32_t & end_page) override;
-    void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) override;
+    void get_log_info(uint16_t list_entry, 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 WARN_IF_UNUSED;
+    int16_t get_log_data(uint16_t list_entry, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override WARN_IF_UNUSED;
     uint16_t get_num_logs() override;
     void start_new_log(void) override;
     uint32_t bufferspace_available() override;
-    void stop_logging(void) override { log_write_started = false; }
+    void stop_logging(void) override;
-    bool logging_enabled() const override { return true; }
+    bool logging_failed() const override;
-    bool logging_failed() const override { return false; }
     bool logging_started(void) const override { return log_write_started; }
 
 protected:
     /* Write a block of data at current offset */
     bool _WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) override;
+    void periodic_1Hz() override;
+    void periodic_10Hz(const uint32_t now) override;
+    bool WritesOK() const override;
+
+    // get the current sector from the current page
+    uint32_t get_sector(uint32_t current_page) {
+        return ((current_page - 1) / df_PagePerSector);
+    }
+
+    // get the current block from the current page
+    uint32_t get_block(uint32_t current_page) {
+        return ((current_page - 1) / df_PagePerBlock);
+    }
+
+    // number of bytes in a page
+    uint32_t df_PageSize;
+    // number of pages in a (generally 64k) block
+    uint16_t df_PagePerBlock;
+    // number of pages in a (generally 4k) sector
+    uint16_t df_PagePerSector;
+    // number of pages on the chip
+    uint32_t df_NumPages;
+    volatile bool log_write_started;
+
+    static const uint16_t page_size_max = 256;
+    uint8_t *buffer;
+    uint32_t last_messagewrite_message_sent;
 
 private:
     /*
@@ -50,18 +76,32 @@ private:
     struct PACKED PageHeader {
         uint32_t FilePage;
         uint16_t FileNumber;
+#if BLOCK_LOG_VALIDATE
+        uint32_t crc;
+#endif
     };
 
+    struct PACKED FileHeader {
+        uint32_t utc_secs;
+    };
+
+    // semaphore to mediate access to the chip
     HAL_Semaphore sem;
+    // semaphore to mediate access to the ring buffer
+    HAL_Semaphore write_sem;
     ByteBuffer writebuf;
 
     // state variables
     uint16_t df_Read_BufferIdx;
     uint32_t df_PageAdr;
     uint32_t df_Read_PageAdr;
+    // file numbers
     uint16_t df_FileNumber;
-    // relative page index of the current file starting at 1
+    uint16_t df_Write_FileNumber;
+    uint32_t df_FileTime;
+    // relative page index of the current read/write file starting at 1
     uint32_t df_FilePage;
+    uint32_t df_Write_FilePage;
     // page to wipe from in the case of corruption
     uint32_t df_EraseFrom;
 
@@ -69,7 +109,20 @@ private:
     bool adding_fmt_headers;
 
     // are we waiting on an erase to finish?
-    bool erase_started;
+    volatile bool erase_started;
+    // were we logging before the erase started?
+    volatile bool new_log_pending;
+    // latch to make sure we only write out the full message once
+    volatile bool chip_full;
+    // io thread health
+    volatile uint32_t io_timer_heartbeat;
+    uint8_t warning_decimation_counter;
+
+    volatile enum class StatusMessage {
+        NONE,
+        ERASE_COMPLETE,
+        RECOVERY_COMPLETE,
+    } status_msg;
 
     // read size bytes of data to a page. The caller must ensure that
     // the data fits within the page, otherwise it will wrap to the
@@ -82,48 +135,27 @@ private:
 
     // internal high level functions
     int16_t get_log_data_raw(uint16_t log_num, uint32_t page, uint32_t offset, uint16_t len, uint8_t *data) WARN_IF_UNUSED;
-    void StartRead(uint32_t PageAdr);
+    // read from the page address and return the file number at that location
+    uint16_t StartRead(uint32_t PageAdr);
+    // read the headers at the current read point returning the file number
+    uint16_t ReadHeaders();
     uint32_t find_last_page(void);
     uint32_t find_last_page_of_log(uint16_t log_number);
-    bool check_wrapped(void);
+    bool is_wrapped(void);
     void StartWrite(uint32_t PageAdr);
     void FinishWrite(void);
 
     // Read methods
     bool ReadBlock(void *pBuffer, uint16_t size);
 
+    void StartLogFile(uint16_t FileNumber);
     // file numbers
-    void SetFileNumber(uint16_t FileNumber);
     uint16_t GetFileNumber();
 
     void _print_log_formats(AP_HAL::BetterStream *port);
 
     // callback on IO thread
+    bool io_thread_alive() const;
     void io_timer(void);
-
+    void write_log_page();
-protected:
-    // number of bytes in a page
-    uint32_t df_PageSize;
-    // number of pages in a (generally 64k) block
-    uint16_t df_PagePerBlock;
-    // number of pages in a (generally 4k) sector
-    uint16_t df_PagePerSector;
-    // number of pages on the chip
-    uint32_t df_NumPages;
-    bool log_write_started;
-
-    // get the current sector from the current page
-    uint32_t get_sector(uint32_t current_page) {
-        return ((current_page - 1) / df_PagePerSector);
-    }
-
-    // get the current block from the current page
-    uint32_t get_block(uint32_t current_page) {
-        return ((current_page - 1) / df_PagePerBlock);
-    }
-
-    static const uint16_t page_size_max = 256;
-    uint8_t *buffer;
-
-    bool WritesOK() const override;
 };

libraries/AP_Logger/AP_Logger_DataFlash.cpp

@@ -157,7 +157,6 @@ bool AP_Logger_DataFlash::getSectorCount(void)
     df_NumPages = blocks * df_PagePerBlock;
     erase_cmd = JEDEC_BLOCK64_ERASE;
 
-    hal.scheduler->delay(2000);
     printf("SPI Flash 0x%08x found pages=%u erase=%uk\n",
            id, df_NumPages, (df_PagePerBlock * (uint32_t)df_PageSize)/1024);
     return true;
@@ -302,6 +301,9 @@ void AP_Logger_DataFlash::WriteEnable(void)
 
 void AP_Logger_DataFlash::flash_test()
 {
+    // wait for the chip to be ready, this has been moved from Init()
+    hal.scheduler->delay(2000);
+
     for (uint8_t i=1; i<=20; i++) {
         printf("Flash fill %u\n", i);
         if (i % df_PagePerBlock == 0) {

libraries/AP_Logger/AP_Logger_File.cpp

@@ -27,7 +27,6 @@
 
 extern const AP_HAL::HAL& hal;
 
-#define MAX_LOG_FILES 500U
 #define LOGGER_PAGE_SIZE 1024UL
 
 #ifndef HAL_LOGGER_WRITE_CHUNK_SIZE
@@ -80,8 +79,6 @@ void AP_Logger_File::ensure_log_directory_exists()
 
 void AP_Logger_File::Init()
 {
-    AP_Logger_Backend::Init();
-
     // determine and limit file backend buffersize
     uint32_t bufsize = _front._params.file_bufsize;
     if (bufsize > 64) {
@@ -135,11 +132,7 @@ bool AP_Logger_File::log_exists(const uint16_t lognum) const
 
 void AP_Logger_File::periodic_1Hz()
 {
-    if (_rotate_pending && !logging_enabled()) {
+    AP_Logger_Backend::periodic_1Hz();
-        _rotate_pending = false;
-        // handle log rotation once we stop logging
-        stop_logging();
-    }
 
     if (!io_thread_alive()) {
         if (io_thread_warning_decimation_counter == 0 && _initialised) {
@@ -159,7 +152,6 @@ void AP_Logger_File::periodic_1Hz()
         _write_fd = -1;
         _initialised = false;
     }
-    df_stats_log();
 }
 
 void AP_Logger_File::periodic_fullrate()
@@ -170,7 +162,7 @@ void AP_Logger_File::periodic_fullrate()
 uint32_t AP_Logger_File::bufferspace_available()
 {
     const uint32_t space = _writebuf.space();
-    const uint32_t crit = critical_message_reserved_space();
+    const uint32_t crit = critical_message_reserved_space(_writebuf.get_size());
 
     return (space > crit) ? space - crit : 0;
 }
@@ -496,7 +488,7 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
         const uint32_t now = AP_HAL::millis();
         const bool must_dribble = (now - last_messagewrite_message_sent) > 100;
         if (!must_dribble &&
-            space < non_messagewriter_message_reserved_space()) {
+            space < non_messagewriter_message_reserved_space(_writebuf.get_size())) {
             // this message isn't dropped, it will be sent again...
             semaphore.give();
             return false;
@@ -504,7 +496,7 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
         last_messagewrite_message_sent = now;
     } else {
         // we reserve some amount of space for critical messages:
-        if (!is_critical && space < critical_message_reserved_space()) {
+        if (!is_critical && space < critical_message_reserved_space(_writebuf.get_size())) {
             _dropped++;
             semaphore.give();
             return false;
@@ -520,7 +512,7 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
     }
 
     _writebuf.write((uint8_t*)pBuffer, size);
-    df_stats_gather(size);
+    df_stats_gather(size, _writebuf.space());
     semaphore.give();
     return true;
 }
@@ -604,34 +596,12 @@ uint32_t AP_Logger_File::_get_log_time(const uint16_t log_num)
     return st.st_mtime;
 }
 
-/*
-  convert a list entry number back into a log number (which can then
-  be converted into a filename).  A "list entry number" is a sequence
-  where the oldest log has a number of 1, the second-from-oldest 2,
-  and so on.  Thus the highest list entry number is equal to the
-  number of logs.
-*/
-uint16_t AP_Logger_File::_log_num_from_list_entry(const uint16_t list_entry)
-{
-    uint16_t oldest_log = find_oldest_log();
-    if (oldest_log == 0) {
-        // We don't have any logs...
-        return 0;
-    }
-
-    uint32_t log_num = oldest_log + list_entry - 1;
-    if (log_num > MAX_LOG_FILES) {
-        log_num -= MAX_LOG_FILES;
-    }
-    return (uint16_t)log_num;
-}
-
 /*
   find the number of pages in a log
  */
 void AP_Logger_File::get_log_boundaries(const uint16_t list_entry, uint32_t & start_page, uint32_t & end_page)
 {
-    const uint16_t log_num = _log_num_from_list_entry(list_entry);
+    const uint16_t log_num = log_num_from_list_entry(list_entry);
     if (log_num == 0) {
         // that failed - probably no logs
         start_page = 0;
@@ -652,7 +622,7 @@ int16_t AP_Logger_File::get_log_data(const uint16_t list_entry, const uint16_t p
         return -1;
     }
 
-    const uint16_t log_num = _log_num_from_list_entry(list_entry);
+    const uint16_t log_num = log_num_from_list_entry(list_entry);
     if (log_num == 0) {
         // that failed - probably no logs
         return -1;
@@ -706,7 +676,7 @@ int16_t AP_Logger_File::get_log_data(const uint16_t list_entry, const uint16_t p
  */
 void AP_Logger_File::get_log_info(const uint16_t list_entry, uint32_t &size, uint32_t &time_utc)
 {
-    uint16_t log_num = _log_num_from_list_entry(list_entry);
+    uint16_t log_num = log_num_from_list_entry(list_entry);
     if (log_num == 0) {
         // that failed - probably no logs
         size = 0;
@@ -762,18 +732,6 @@ void AP_Logger_File::stop_logging(void)
     }
 }
 
-void AP_Logger_File::PrepForArming()
-{
-    if (_rotate_pending) {
-        _rotate_pending = false;
-        stop_logging();
-    }
-    if (logging_started()) {
-        return;
-    }
-    start_new_log();
-}
-
 /*
   start writing to a new log file
  */
@@ -1019,16 +977,6 @@ void AP_Logger_File::_io_timer(void)
     hal.util->perf_end(_perf_write);
 }
 
-// this sensor is enabled if we should be logging at the moment
-bool AP_Logger_File::logging_enabled() const
-{
-    if (hal.util->get_soft_armed() ||
-        _front.log_while_disarmed()) {
-        return true;
-    }
-    return false;
-}
-
 bool AP_Logger_File::io_thread_alive() const
 {
     // if the io thread hasn't had a heartbeat in a full seconds then it is dead
@@ -1056,56 +1004,5 @@ bool AP_Logger_File::logging_failed() const
     return false;
 }
 
-
-void AP_Logger_File::vehicle_was_disarmed()
-{
-    if (_front._params.file_disarm_rot) {
-        // rotate our log.  Closing the current one and letting the
-        // logging restart naturally based on log_disarmed should do
-        // the trick:
-        _rotate_pending = true;
-    }
-}
-
-void AP_Logger_File::Write_AP_Logger_Stats_File(const struct df_stats &_stats)
-{
-    struct log_DSF pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_DF_FILE_STATS),
-        time_us         : AP_HAL::micros64(),
-        dropped         : _dropped,
-        blocks          : _stats.blocks,
-        bytes           : _stats.bytes,
-        buf_space_min   : _stats.buf_space_min,
-        buf_space_max   : _stats.buf_space_max,
-        buf_space_avg   : (_stats.blocks) ? (_stats.buf_space_sigma / _stats.blocks) : 0,
-
-    };
-    WriteBlock(&pkt, sizeof(pkt));
-}
-
-void AP_Logger_File::df_stats_gather(const uint16_t bytes_written) {
-    const uint32_t space_remaining = _writebuf.space();
-    if (space_remaining < stats.buf_space_min) {
-        stats.buf_space_min = space_remaining;
-    }
-    if (space_remaining > stats.buf_space_max) {
-        stats.buf_space_max = space_remaining;
-    }
-    stats.buf_space_sigma += space_remaining;
-    stats.bytes += bytes_written;
-    stats.blocks++;
-}
-
-void AP_Logger_File::df_stats_clear() {
-    memset(&stats, '\0', sizeof(stats));
-    stats.buf_space_min = -1;
-}
-
-void AP_Logger_File::df_stats_log() {
-    Write_AP_Logger_Stats_File(stats);
-    df_stats_clear();
-}
-
-
 #endif // HAVE_FILESYSTEM_SUPPORT
 

libraries/AP_Logger/AP_Logger_File.h

@@ -29,7 +29,6 @@ public:
     void EraseAll() override;
 
     // possibly time-consuming preparation handling:
-    bool NeedPrep() override;
     void Prep() override;
 
     /* Write a block of data at current offset */
@@ -43,6 +42,7 @@ public:
     int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override;
     uint16_t get_num_logs() override;
     void start_new_log(void) override;
+    uint16_t find_oldest_log() override;
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
     void flush(void) override;
@@ -51,15 +51,10 @@ public:
     void periodic_fullrate() override;
 
     // this method is used when reporting system status over mavlink
-    bool logging_enabled() const override;
     bool logging_failed() const override;
 
     bool logging_started(void) const override { return _write_fd != -1; }
 
-    void vehicle_was_disarmed() override;
-
-    virtual void PrepForArming() override;
-
 protected:
 
     bool WritesOK() const override;
@@ -85,20 +80,13 @@ private:
     bool io_thread_alive() const;
     uint8_t io_thread_warning_decimation_counter;
 
-    uint16_t _cached_oldest_log;
-
-    // should we rotate when we next stop logging
-    bool _rotate_pending;
-
-    uint16_t _log_num_from_list_entry(const uint16_t list_entry);
-
     // possibly time-consuming preparations handling
     void Prep_MinSpace();
-    uint16_t find_oldest_log();
     int64_t disk_space_avail();
     int64_t disk_space();
 
     void ensure_log_directory_exists();
+    bool NeedPrep();
 
     bool file_exists(const char *filename) const;
     bool log_exists(const uint16_t lognum) const;
@@ -120,26 +108,6 @@ private:
 
     void _io_timer(void);
 
-    uint32_t critical_message_reserved_space() const {
-        // possibly make this a proportional to buffer size?
-        uint32_t ret = 1024;
-        if (ret > _writebuf.get_size()) {
-            // in this case you will only get critical messages
-            ret = _writebuf.get_size();
-        }
-        return ret;
-    };
-    uint32_t non_messagewriter_message_reserved_space() const {
-        // possibly make this a proportional to buffer size?
-        uint32_t ret = 1024;
-        if (ret >= _writebuf.get_size()) {
-            // need to allow messages out from the messagewriters.  In
-            // this case while you have a messagewriter you won't get
-            // any other messages.  This should be a corner case!
-            ret = 0;
-        }
-        return ret;
-    };
     uint32_t last_messagewrite_message_sent;
 
     // free-space checks; filling up SD cards under NuttX leads to
@@ -163,21 +131,6 @@ private:
     AP_HAL::Util::perf_counter_t  _perf_overruns;
 
     const char *last_io_operation = "";
-
-    struct df_stats {
-        uint16_t blocks;
-        uint32_t bytes;
-        uint32_t buf_space_min;
-        uint32_t buf_space_max;
-        uint32_t buf_space_sigma;
-    };
-    struct df_stats stats;
-
-    void Write_AP_Logger_Stats_File(const struct df_stats &_stats);
-    void df_stats_gather(uint16_t bytes_written);
-    void df_stats_log();
-    void df_stats_clear();
-
 };
 
 #endif // HAVE_FILESYSTEM_SUPPORT

libraries/AP_Logger/AP_Logger_MAVLink.cpp

@@ -26,8 +26,6 @@ extern const AP_HAL::HAL& hal;
 // initialisation
 void AP_Logger_MAVLink::Init()
 {
-    AP_Logger_Backend::Init();
-
     _blocks = nullptr;
     while (_blockcount >= 8) { // 8 is a *magic* number
         _blocks = (struct dm_block *) calloc(_blockcount, sizeof(struct dm_block));
@@ -536,11 +534,6 @@ void AP_Logger_MAVLink::periodic_1Hz()
     stats_log();
 }
 
-void AP_Logger_MAVLink::periodic_fullrate()
-{
-    push_log_blocks();
-}
-
 //TODO: handle full txspace properly
 bool AP_Logger_MAVLink::send_log_block(struct dm_block &block)
 {

libraries/AP_Logger/AP_Logger_MAVLink.h

@@ -49,7 +49,7 @@ public:
     // erase handling
     void EraseAll() override {}
 
-    bool NeedPrep() override { return false; }
+    void PrepForArming() override {}
     void Prep() override { }
 
     // high level interface
@@ -59,12 +59,12 @@ public:
     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 push_log_blocks() override;
-
     void remote_log_block_status_msg(const mavlink_channel_t chan, const mavlink_message_t& msg) override;
+    void vehicle_was_disarmed() override {}
 
 protected:
 
+    void push_log_blocks() override;
     bool WritesOK() const override;
 
 private:
@@ -157,7 +157,6 @@ private:
 
     void periodic_10Hz(uint32_t now) override;
     void periodic_1Hz() override;
-    void periodic_fullrate() override;
     
     void stats_init();
     void stats_reset();

libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp

@@ -77,7 +77,6 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, const mavlink_message
     mavlink_msg_log_request_list_decode(&msg, &packet);
 
     _log_num_logs = get_num_logs();
-    uint16_t last_log = find_last_log();
 
     if (_log_num_logs == 0) {
         _log_next_list_entry = 0;
@@ -86,11 +85,11 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, const mavlink_message
         _log_next_list_entry = packet.start;
         _log_last_list_entry = packet.end;
 
-        if (_log_last_list_entry > last_log) {
+        if (_log_last_list_entry > _log_num_logs) {
-            _log_last_list_entry = last_log;
+            _log_last_list_entry = _log_num_logs;
         }
         if (_log_next_list_entry < 1) {
-            _log_next_list_entry = last_log - _log_num_logs + 1;
+            _log_next_list_entry = 1;
         }
     }
 
@@ -126,8 +125,7 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, const mavlink_message
     if (transfer_activity != TransferActivity::SENDING || _log_num_data != packet.id) {
 
         uint16_t num_logs = get_num_logs();
-        uint16_t last_log = find_last_log();
+        if (packet.id > num_logs || packet.id < 1) {
-        if (packet.id > last_log || packet.id < (last_log - num_logs + 1)) {
             // request for an invalid log; cancel any current download
             transfer_activity = TransferActivity::IDLE;
             return;
@@ -293,25 +291,27 @@ bool AP_Logger::handle_log_send_data()
         return false;
     }
 
-    int16_t ret = 0;
+    int16_t nbytes = 0;
     uint32_t len = _log_data_remaining;
 	mavlink_log_data_t packet;
 
     if (len > MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
         len = MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;
     }
-    ret = get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
+
-    if (ret < 0) {
+    nbytes = get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
+
+    if (nbytes < 0) {
         // report as EOF on error
-        ret = 0;
+        nbytes = 0;
     }
-    if (ret < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
+    if (nbytes < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
-        memset(&packet.data[ret], 0, MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN-ret);
+        memset(&packet.data[nbytes], 0, MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN-nbytes);
     }
 
     packet.ofs = _log_data_offset;
     packet.id = _log_num_data;
-    packet.count = ret;
+    packet.count = nbytes;
     _mav_finalize_message_chan_send(_log_sending_link->get_chan(),
                                     MAVLINK_MSG_ID_LOG_DATA,
                                     (const char *)&packet,
@@ -319,9 +319,9 @@ bool AP_Logger::handle_log_send_data()
                                     MAVLINK_MSG_ID_LOG_DATA_LEN,
                                     MAVLINK_MSG_ID_LOG_DATA_CRC);
 
-    _log_data_offset += len;
+    _log_data_offset += nbytes;
-    _log_data_remaining -= len;
+    _log_data_remaining -= nbytes;
-    if (ret < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN || _log_data_remaining == 0) {
+    if (nbytes < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN || _log_data_remaining == 0) {
         transfer_activity = TransferActivity::IDLE;
         _log_sending_link = nullptr;
     }

libraries/AP_Logger/AP_Logger_SITL.cpp

@@ -18,6 +18,7 @@
 
 #define DF_PAGE_SIZE 256UL
 #define DF_PAGE_PER_SECTOR 16 // 4k sectors
+#define DF_PAGE_PER_BLOCK 256 // 4k sectors
 #define DF_NUM_PAGES 65536UL
 
 #define ERASE_TIME_MS 10000
@@ -30,17 +31,15 @@ void AP_Logger_SITL::Init()
         flash_fd = open(filename, O_RDWR|O_CLOEXEC, 0777);
         if (flash_fd == -1) {
             flash_fd = open(filename, O_RDWR | O_CREAT | O_CLOEXEC, 0777);
-            StartErase();
-            erase_started_ms = 0;
-        }
             if (ftruncate(flash_fd, DF_PAGE_SIZE*uint32_t(DF_NUM_PAGES)) != 0) {
                 AP_HAL::panic("Failed to create %s", filename);
             }
         }
+    }
 
     df_PageSize = DF_PAGE_SIZE;
     df_PagePerSector = DF_PAGE_PER_SECTOR;
-    df_PagePerBlock = DF_PAGE_PER_SECTOR;
+    df_PagePerBlock = DF_PAGE_PER_BLOCK;
     df_NumPages = DF_NUM_PAGES;
 
     AP_Logger_Block::Init();
@@ -69,21 +68,25 @@ void AP_Logger_SITL::BufferToPage(uint32_t PageAdr)
 
 void AP_Logger_SITL::SectorErase(uint32_t SectorAdr)
 {
-    uint8_t fill[DF_PAGE_SIZE*DF_PAGE_PER_SECTOR];
+    uint8_t fill[DF_PAGE_SIZE*DF_PAGE_PER_BLOCK];
     memset(fill, 0xFF, sizeof(fill));
-    if (pwrite(flash_fd, fill, sizeof(fill), SectorAdr*DF_PAGE_PER_SECTOR*DF_PAGE_SIZE) != sizeof(fill)) {
+    if (pwrite(flash_fd, fill, sizeof(fill), SectorAdr*DF_PAGE_PER_BLOCK*DF_PAGE_SIZE) != sizeof(fill)) {
         printf("Failed sector erase");
     }
 }
 
 void AP_Logger_SITL::Sector4kErase(uint32_t SectorAdr)
 {
-    SectorErase(SectorAdr);
+    uint8_t fill[DF_PAGE_SIZE*DF_PAGE_PER_SECTOR];
+    memset(fill, 0xFF, sizeof(fill));
+    if (pwrite(flash_fd, fill, sizeof(fill), SectorAdr*DF_PAGE_PER_SECTOR*DF_PAGE_SIZE) != sizeof(fill)) {
+        printf("Failed sector 4k erase");
+    }
 }
 
 void AP_Logger_SITL::StartErase()
 {
-    for (uint32_t i=0; i<DF_NUM_PAGES/DF_PAGE_PER_SECTOR; i++) {
+    for (uint32_t i=0; i<DF_NUM_PAGES/DF_PAGE_PER_BLOCK; i++) {
         SectorErase(i);
     }
     erase_started_ms = AP_HAL::millis();

libraries/AP_Logger/LoggerMessageWriter.cpp

@@ -6,7 +6,10 @@
 #include "ap_version.h"
 #undef FORCE_VERSION_H_INCLUDE
 
-#define MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US 200
+// the message writers are mainly called at the end of periodic_fullrate() with a budget of 300us
+// until they are finished calls to Write() will fail so we want to take up as much of
+// the budget as possible in order that logging can begin in earnest as early as possible
+#define MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US 50
 
 extern const AP_HAL::HAL& hal;
 
@@ -42,7 +45,7 @@ void LoggerMessageWriter_DFLogStart::process()
     case Stage::FORMATS:
         // write log formats so the log is self-describing
         while (next_format_to_send < _logger_backend->num_types()) {
-            if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) {
+            if (AP::scheduler().time_available_usec() == 0) { // write out the FMT messages as fast as we can
                 return;
             }
             if (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send))) {

libraries/AP_Logger/examples/AP_Logger_test/AP_Logger_test.cpp

@@ -56,7 +56,14 @@ void AP_LoggerTest::setup(void)
     logger.Init(log_structure, ARRAY_SIZE(log_structure));
     logger.set_vehicle_armed(true);
     logger.Write_Message("AP_Logger Test");
-
+#ifdef DEBUG_RATES
+    hal.console->printf("| Type | Size | 10Hz(bs) | 25Hz(bs) | 400Hz(Kbs) |\n");
+    for (uint16_t i = 0; i < ARRAY_SIZE(log_structure); i++) {
+        LogStructure log = log_structure[i];
+        hal.console->printf("| %-6s | %3d | %4d | %4d | %2dk |\n", log.name, log.msg_len,
+            log.msg_len * 10, log.msg_len * 25, log.msg_len * 400 / 1000);
+    }
+#endif
     // Test
     hal.scheduler->delay(20);