diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 4c869840f8..09d1d9e94e 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -4,6 +4,8 @@ #include "AP_Logger_File.h" #include "AP_Logger_File_sd.h" +#include "AP_Logger_SITL.h" +#include "AP_Logger_DataFlash.h" #include "AP_Logger_MAVLink.h" #include #if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT @@ -28,6 +30,7 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = { // @DisplayName: AP_Logger Backend Storage type // @Description: 0 for None, 1 for File, 2 for dataflash mavlink, 3 for both file and dataflash // @Values: 0:None,1:File,2:MAVLink,3:BothFileAndMAVLink + // @Bitmask: 0:File,1:MAVLink,2:Block // @User: Standard AP_GROUPINFO("_BACKEND_TYPE", 0, AP_Logger, _params.backend_types, DATAFLASH_BACKEND_FILE), @@ -97,8 +100,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types) #if defined(HAL_BOARD_LOG_DIRECTORY) #if HAL_OS_POSIX_IO || HAL_OS_FATFS_IO - if (_params.backend_types == DATAFLASH_BACKEND_FILE || - _params.backend_types == DATAFLASH_BACKEND_BOTH) { + if (_params.backend_types & DATAFLASH_BACKEND_FILE) { LoggerMessageWriter_DFLogStart *message_writer = new LoggerMessageWriter_DFLogStart(); if (message_writer != nullptr) { @@ -112,34 +114,11 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types) _next_backend++; } } - #elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT - - if (_params.backend_types == DATAFLASH_BACKEND_FILE || - _params.backend_types == DATAFLASH_BACKEND_BOTH) { - - LoggerMessageWriter_DFLogStart *message_writer = - new LoggerMessageWriter_DFLogStart(); - if (message_writer != nullptr) { - - #if defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS) - backends[_next_backend] = new AP_Logger_File(*this, message_writer, HAL_BOARD_LOG_DIRECTORY); - #else - backends[_next_backend] = new AP_Logger_Revo(*this, message_writer); // restore dataflash logs - #endif - } - - if (backends[_next_backend] == nullptr) { - printf("Unable to open AP_Logger_Revo"); - } else { - _next_backend++; - } - } #endif #endif // HAL_BOARD_LOG_DIRECTORY #if DATAFLASH_MAVLINK_SUPPORT - if (_params.backend_types == DATAFLASH_BACKEND_MAVLINK || - _params.backend_types == DATAFLASH_BACKEND_BOTH) { + if (_params.backend_types & DATAFLASH_BACKEND_MAVLINK) { if (_next_backend == DATAFLASH_MAX_BACKENDS) { AP_HAL::panic("Too many backends"); return; @@ -158,6 +137,44 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types) } #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (_params.backend_types & DATAFLASH_BACKEND_BLOCK) { + if (_next_backend == DATAFLASH_MAX_BACKENDS) { + AP_HAL::panic("Too many backends"); + return; + } + LoggerMessageWriter_DFLogStart *message_writer = + new LoggerMessageWriter_DFLogStart(); + if (message_writer != nullptr) { + backends[_next_backend] = new AP_Logger_SITL(*this, message_writer); + } + if (backends[_next_backend] == nullptr) { + hal.console->printf("Unable to open AP_Logger_SITL"); + } else { + _next_backend++; + } + } +#endif + +#ifdef HAL_LOGGING_DATAFLASH + if (_params.backend_types & DATAFLASH_BACKEND_BLOCK) { + if (_next_backend == DATAFLASH_MAX_BACKENDS) { + AP_HAL::panic("Too many backends"); + return; + } + LoggerMessageWriter_DFLogStart *message_writer = + new LoggerMessageWriter_DFLogStart(); + if (message_writer != nullptr) { + backends[_next_backend] = new AP_Logger_DataFlash(*this, message_writer); + } + if (backends[_next_backend] == nullptr) { + hal.console->printf("Unable to open AP_Logger_DataFlash"); + } else { + _next_backend++; + } + } +#endif + for (uint8_t i=0; i<_next_backend; i++) { backends[i]->Init(); } @@ -556,7 +573,7 @@ uint16_t AP_Logger::find_last_log() const { } return backends[0]->find_last_log(); } -void AP_Logger::get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) { +void AP_Logger::get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) { if (_next_backend == 0) { return; } diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 0df4ef4d23..5d578ad249 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -31,10 +31,10 @@ class AP_Logger_Backend; enum AP_Logger_Backend_Type { - DATAFLASH_BACKEND_NONE = 0, - DATAFLASH_BACKEND_FILE = 1, - DATAFLASH_BACKEND_MAVLINK = 2, - DATAFLASH_BACKEND_BOTH = 3, + DATAFLASH_BACKEND_NONE = 0, + DATAFLASH_BACKEND_FILE = (1<<0), + DATAFLASH_BACKEND_MAVLINK = (1<<1), + DATAFLASH_BACKEND_BLOCK = (1<<2), }; // fwd declarations to avoid include errors @@ -75,7 +75,7 @@ public: // high level interface uint16_t find_last_log() const; - void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page); + void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page); uint16_t get_num_logs(void); void setVehicle_Startup_Writer(vehicle_startup_message_Writer writer); @@ -363,7 +363,7 @@ private: uint32_t _log_data_remaining; // start page of log data - uint16_t _log_data_page; + uint32_t _log_data_page; GCS_MAVLINK *_log_sending_link; diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index c953a8a989..10e28e7f5b 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -38,7 +38,7 @@ public: // high level interface virtual uint16_t find_last_log() = 0; - virtual void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) = 0; + virtual void get_log_boundaries(uint16_t log_num, 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 int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) = 0; virtual uint16_t get_num_logs() = 0; diff --git a/libraries/AP_Logger/AP_Logger_Block.cpp b/libraries/AP_Logger/AP_Logger_Block.cpp new file mode 100644 index 0000000000..6457890d3f --- /dev/null +++ b/libraries/AP_Logger/AP_Logger_Block.cpp @@ -0,0 +1,626 @@ +/* + block based logging, for boards with flash logging + */ + +#include "AP_Logger_Block.h" + +#include +#include + +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 0x19012019 + +AP_Logger_Block::AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) : + writebuf(0), + AP_Logger_Backend(front, writer) +{ + 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"); + } +} + +// init is called after backend init +void AP_Logger_Block::Init(void) +{ + if (CardInserted()) { + // reserve space for version in last sector + df_NumPages -= df_PagePerSector; + + // determine and limit file backend buffersize + uint32_t bufsize = _front._params.file_bufsize; + if (bufsize > 64) { + bufsize = 64; + } + bufsize *= 1024; + + // If we can't allocate the full size, try to reduce it until we can allocate it + while (!writebuf.set_size(bufsize) && bufsize >= df_PageSize * df_PagePerSector) { + hal.console->printf("AP_Logger_Block: Couldn't set buffer size to=%u\n", (unsigned)bufsize); + bufsize >>= 1; + } + + if (!writebuf.get_size()) { + hal.console->printf("Out of memory for logging\n"); + return; + } + + hal.console->printf("AP_Logger_Block: buffer size=%u\n", (unsigned)bufsize); + _initialised = true; + } + + 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() +{ + // because AP_Logger_Block devices are ring buffers, we *always* + // have room... + return df_NumPages * df_PageSize; +} + +// *** DATAFLASH PUBLIC FUNCTIONS *** +void AP_Logger_Block::StartWrite(uint32_t PageAdr) +{ + df_PageAdr = PageAdr; + log_write_started = true; +} + +void AP_Logger_Block::FinishWrite(void) +{ + // Write Buffer to flash + BufferToPage(df_PageAdr); + df_PageAdr++; + + // If we reach the end of the memory, start from the beginning + if (df_PageAdr > df_NumPages) { + df_PageAdr = 1; + } + + // when starting a new sector, erase it + if ((df_PageAdr-1) % df_PagePerSector == 0) { + SectorErase(df_PageAdr / df_PagePerSector); + } +} + +bool AP_Logger_Block::WritesOK() const +{ + if (!CardInserted()) { + return false; + } + return true; +} + +bool AP_Logger_Block::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) +{ + // is_critical is ignored - we're a ring buffer and never run out + // of space. possibly if we do more complicated bandwidth + // limiting we can reserve bandwidth based on is_critical + if (!WritesOK()) { + return false; + } + + if (! WriteBlockCheckStartupMessages()) { + return false; + } + + if (writebuf.space() < size) { + // no room in buffer + return false; + } + + writebuf.write((uint8_t*)pBuffer, size); + return true; +} + + +void AP_Logger_Block::StartRead(uint32_t PageAdr) +{ + df_Read_PageAdr = PageAdr; + + // copy flash page to buffer + if (erase_started) { + memset(buffer, 0xff, df_PageSize); + } else { + PageToBuffer(df_Read_PageAdr); + } + + // 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; + df_Read_BufferIdx = sizeof(ph); +} + +bool AP_Logger_Block::ReadBlock(void *pBuffer, uint16_t size) +{ + if (erase_started) { + return false; + } + while (size > 0) { + uint16_t n = df_PageSize - df_Read_BufferIdx; + if (n > size) { + n = size; + } + + if (!BlockRead(df_Read_BufferIdx, pBuffer, n)) { + return false; + } + size -= n; + pBuffer = (void *)(n + (uintptr_t)pBuffer); + + df_Read_BufferIdx += n; + + if (df_Read_BufferIdx == df_PageSize) { + df_Read_PageAdr++; + if (df_Read_PageAdr > df_NumPages) { + df_Read_PageAdr = 1; + } + if (erase_started) { + memset(buffer, 0xff, df_PageSize); + } else { + PageToBuffer(df_Read_PageAdr); + } + + // We are starting a new page - read FileNumber and FilePage + struct PageHeader ph; + 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) +{ + df_FileNumber = FileNumber; + df_FilePage = 1; +} + +uint16_t AP_Logger_Block::GetFileNumber() +{ + return df_FileNumber; +} + +void AP_Logger_Block::EraseAll() +{ + WITH_SEMAPHORE(sem); + + if (erase_started) { + // already erasing + return; + } + + 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; + } + if (NeedErase()) { + EraseAll(); + } +} + +/* + * we need to erase if the logging format has changed + */ +bool AP_Logger_Block::NeedErase(void) +{ + uint32_t version = 0; + StartRead(df_NumPages+1); // last page + + BlockRead(0, &version, sizeof(version)); + StartRead(1); + if (version == DF_LOGGING_FORMAT) { + return false; + } + return true; +} + +/** + get raw data from a log + */ +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); + + 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 = 1 + page - df_NumPages; + } + } + if (log_write_started || df_Read_PageAdr != page) { + StartRead(page); + } + + df_Read_BufferIdx = offset + sizeof(struct PageHeader); + if (!ReadBlock(data, len)) { + return -1; + } + + return (int16_t)len; +} + +/** + 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) +{ + WITH_SEMAPHORE(sem); + 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); + 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) { + ret += get_log_data_raw(log_num, page, offset, len, data); + } + + return ret; +} + + +// This function determines the number of whole or partial log files in the AP_Logger +// Wholly overwritten files are (of course) lost. +uint16_t AP_Logger_Block::get_num_logs(void) +{ + WITH_SEMAPHORE(sem); + uint32_t lastpage; + uint32_t last; + uint32_t first; + + if (!CardInserted() || find_last_page() == 1) { + return 0; + } + + StartRead(1); + + if (GetFileNumber() == 0xFFFF) { + return 0; + } + + lastpage = find_last_page(); + StartRead(lastpage); + last = GetFileNumber(); + StartRead(lastpage + 2); + if (GetFileNumber() == 0xFFFF) { + StartRead(((lastpage >> 8) + 1) << 8); // next sector + } + first = GetFileNumber(); + if (first > last) { + StartRead(1); + first = GetFileNumber(); + } + + if (last == first) { + return 1; + } + + return (last - first + 1); +} + + +// This function starts a new log file in the AP_Logger +uint16_t AP_Logger_Block::start_new_log(void) +{ + WITH_SEMAPHORE(sem); + uint32_t last_page = find_last_page(); + + StartRead(last_page); + + if (find_last_log() == 0 || GetFileNumber() == 0xFFFF) { + SetFileNumber(1); + StartWrite(1); + return 1; + } + + uint16_t new_log_num; + + // Check for log of length 1 page and suppress + if (df_FilePage <= 1) { + new_log_num = GetFileNumber(); + // Last log too short, reuse its number + // and overwrite it + SetFileNumber(new_log_num); + StartWrite(last_page); + } else { + new_log_num = GetFileNumber()+1; + if (last_page == 0xFFFF) { + last_page=0; + } + SetFileNumber(new_log_num); + StartWrite(last_page + 1); + } + return new_log_num; +} + +// 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) +{ + WITH_SEMAPHORE(sem); + uint16_t num = get_num_logs(); + uint32_t look; + + if (num == 1) { + StartRead(df_NumPages); + if (GetFileNumber() == 0xFFFF) { + start_page = 1; + 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; + } + } else { + if (log_num == find_last_log() - num + 1) { + start_page = find_last_page() + 1; + } else { + look = log_num-1; + do { + start_page = find_last_page_of_log(look) + 1; + look--; + } 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) +{ + StartRead(df_NumPages); + return GetFileNumber() != 0xFFFF; +} + + +// This funciton finds the last log number +uint16_t AP_Logger_Block::find_last_log(void) +{ + WITH_SEMAPHORE(sem); + uint32_t last_page = find_last_page(); + StartRead(last_page); + return GetFileNumber(); +} + +// This function finds the last page of the last file +uint32_t AP_Logger_Block::find_last_page(void) +{ + uint32_t look; + uint32_t bottom = 1; + uint32_t top = df_NumPages; + uint32_t look_hash; + uint32_t bottom_hash; + uint32_t top_hash; + + WITH_SEMAPHORE(sem); + + StartRead(bottom); + bottom_hash = ((int32_t)GetFileNumber()<<16) | df_FilePage; + + while (top-bottom > 1) { + look = (top+bottom)/2; + StartRead(look); + look_hash = (int32_t)GetFileNumber()<<16 | df_FilePage; + if (look_hash >= 0xFFFF0000) { + look_hash = 0; + } + + if (look_hash < bottom_hash) { + // move down + top = look; + } else { + // move up + bottom = look; + bottom_hash = look_hash; + } + } + + StartRead(top); + top_hash = ((int32_t)GetFileNumber()<<16) | df_FilePage; + if (top_hash >= 0xFFFF0000) { + top_hash = 0; + } + if (top_hash > bottom_hash) { + return top; + } + + return bottom; +} + +// This function finds the last page of a particular log file +uint32_t AP_Logger_Block::find_last_page_of_log(uint16_t log_number) +{ + uint32_t look; + uint32_t bottom; + uint32_t top; + uint32_t look_hash; + uint32_t check_hash; + + WITH_SEMAPHORE(sem); + + if (check_wrapped()) { + StartRead(1); + bottom = GetFileNumber(); + if (bottom > log_number) { + bottom = find_last_page(); + top = df_NumPages; + } else { + bottom = 1; + top = find_last_page(); + } + } else { + bottom = 1; + top = find_last_page(); + } + + check_hash = (int32_t)log_number<<16 | 0xFFFF; + + while (top-bottom > 1) { + look = (top+bottom)/2; + StartRead(look); + look_hash = (int32_t)GetFileNumber()<<16 | df_FilePage; + if (look_hash >= 0xFFFF0000) { + look_hash = 0; + } + + if (look_hash > check_hash) { + // move down + top = look; + } else { + // move up + bottom = look; + } + } + + StartRead(top); + if (GetFileNumber() == log_number) { + return top; + } + + StartRead(bottom); + if (GetFileNumber() == log_number) { + return bottom; + } + + return 0; +} + + +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); + if (end >= start) { + size = (end + 1 - start) * (uint32_t)df_PageSize; + } else { + size = (df_NumPages + end - start) * (uint32_t)df_PageSize; + } + time_utc = 0; +} + + +void AP_Logger_Block::PrepForArming() +{ + if (logging_started()) { + return; + } + start_new_log(); +} + +// read size bytes of data from the buffer +bool AP_Logger_Block::BlockRead(uint16_t IntPageAdr, void *pBuffer, uint16_t size) +{ + memcpy(pBuffer, &buffer[IntPageAdr], size); + return true; +} + +/* + IO timer running on IO thread + */ +void AP_Logger_Block::io_timer(void) +{ + if (!_initialised) { + return; + } + + if (erase_started) { + if (InErase()) { + return; + } + // write the logging format in the last page + StartWrite(df_NumPages+1); + uint32_t version = DF_LOGGING_FORMAT; + memset(buffer, 0, df_PageSize); + memcpy(buffer, &version, sizeof(version)); + FinishWrite(); + erase_started = false; + } + + if (!CardInserted() || !log_write_started) { + return; + } + + while (writebuf.available() >= df_PageSize - sizeof(struct PageHeader)) { + WITH_SEMAPHORE(sem); + struct PageHeader ph; + ph.FileNumber = df_FileNumber; + ph.FilePage = df_FilePage; + memcpy(buffer, &ph, sizeof(ph)); + writebuf.read(&buffer[sizeof(ph)], df_PageSize - sizeof(ph)); + FinishWrite(); + df_FilePage++; + } +} diff --git a/libraries/AP_Logger/AP_Logger_Block.h b/libraries/AP_Logger/AP_Logger_Block.h new file mode 100644 index 0000000000..e29c4dfc9f --- /dev/null +++ b/libraries/AP_Logger/AP_Logger_Block.h @@ -0,0 +1,110 @@ +/* + AP_Logger logging - block oriented variant + */ +#pragma once + +#include "AP_Logger_Backend.h" + +class AP_Logger_Block : public AP_Logger_Backend { +public: + AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer); + + virtual void Init(void) override; + virtual bool CardInserted(void) const override = 0; + + // 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_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; + uint16_t get_num_logs() override; + uint16_t start_new_log(void) override; + uint32_t bufferspace_available() override; + void stop_logging(void) override { log_write_started = false; } + bool logging_enabled() const override { return true; } + 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; + +private: + /* + functions implemented by the board specific backends + */ + virtual void BufferToPage(uint32_t PageAdr) = 0; + virtual void PageToBuffer(uint32_t PageAdr) = 0; + virtual void SectorErase(uint32_t PageAdr) = 0; + virtual void StartErase() = 0; + virtual bool InErase() = 0; + + struct PageHeader { + uint16_t FileNumber; + uint16_t FilePage; + }; + + HAL_Semaphore_Recursive sem; + ByteBuffer writebuf; + + // state variables + uint16_t df_Read_BufferIdx; + uint32_t df_PageAdr; + uint32_t df_Read_PageAdr; + uint16_t df_FileNumber; + uint32_t df_FilePage; + + // offset from adding FMT messages to log data + bool adding_fmt_headers; + + // are we waiting on an erase to finish? + bool erase_started; + + // 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 + // start of the page + bool BlockRead(uint16_t IntPageAdr, void *pBuffer, uint16_t size); + + // erase handling + bool NeedErase(void); + + // 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); + void StartRead(uint32_t PageAdr); + uint32_t find_last_page(void); + uint32_t find_last_page_of_log(uint16_t log_number); + bool check_wrapped(void); + void StartWrite(uint32_t PageAdr); + void FinishWrite(void); + + // Read methods + bool ReadBlock(void *pBuffer, uint16_t size); + + // file numbers + void SetFileNumber(uint16_t FileNumber); + uint16_t GetFileNumber(); + + void _print_log_formats(AP_HAL::BetterStream *port); + + // callback on IO thread + void io_timer(void); + +protected: + // page handling + uint32_t df_PageSize; + uint16_t df_PagePerSector; + uint32_t df_NumPages; + bool log_write_started; + + static const uint16_t page_size_max = 256; + uint8_t *buffer; + + bool WritesOK() const override; +}; diff --git a/libraries/AP_Logger/AP_Logger_DataFlash.cpp b/libraries/AP_Logger/AP_Logger_DataFlash.cpp new file mode 100644 index 0000000000..020282e179 --- /dev/null +++ b/libraries/AP_Logger/AP_Logger_DataFlash.cpp @@ -0,0 +1,303 @@ +/* + logging to a DataFlash block based storage device on SPI +*/ + + +#include + +#ifdef HAL_LOGGING_DATAFLASH + +#include "AP_Logger_DataFlash.h" +#include + +extern const AP_HAL::HAL& hal; + +#define JEDEC_WRITE_ENABLE 0x06 +#define JEDEC_WRITE_DISABLE 0x04 +#define JEDEC_READ_STATUS 0x05 +#define JEDEC_WRITE_STATUS 0x01 +#define JEDEC_READ_DATA 0x03 +#define JEDEC_FAST_READ 0x0b +#define JEDEC_DEVICE_ID 0x9F +#define JEDEC_PAGE_WRITE 0x02 + +#define JEDEC_BULK_ERASE 0xC7 +#define JEDEC_SECTOR4_ERASE 0x20 // 4k erase +#define JEDEC_BLOCK32_ERASE 0x52 // 32K erase +#define JEDEC_BLOCK64_ERASE 0xD8 // 64K erase + +#define JEDEC_STATUS_BUSY 0x01 +#define JEDEC_STATUS_WRITEPROTECT 0x02 +#define JEDEC_STATUS_BP0 0x04 +#define JEDEC_STATUS_BP1 0x08 +#define JEDEC_STATUS_BP2 0x10 +#define JEDEC_STATUS_TP 0x20 +#define JEDEC_STATUS_SEC 0x40 +#define JEDEC_STATUS_SRP0 0x80 + +/* + flash device IDs taken from betaflight flash_m25p16.c + + Format is manufacturer, memory type, then capacity +*/ +#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016 +#define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017 +#define JEDEC_ID_MACRONIX_MX25L25635E 0xC22019 +#define JEDEC_ID_MICRON_M25P16 0x202015 +#define JEDEC_ID_MICRON_N25Q064 0x20BA17 +#define JEDEC_ID_MICRON_N25Q128 0x20ba18 +#define JEDEC_ID_WINBOND_W25Q16 0xEF4015 +#define JEDEC_ID_WINBOND_W25Q32 0xEF4016 +#define JEDEC_ID_WINBOND_W25Q64 0xEF4017 +#define JEDEC_ID_WINBOND_W25Q128 0xEF4018 +#define JEDEC_ID_WINBOND_W25Q256 0xEF4019 +#define JEDEC_ID_CYPRESS_S25FL128L 0x016018 + +void AP_Logger_DataFlash::Init() +{ + dev = hal.spi->get_device("dataflash"); + if (!dev) { + AP_HAL::panic("PANIC: AP_Logger SPIDeviceDriver not found"); + return; + } + + dev_sem = dev->get_semaphore(); + + if (!getSectorCount()) { + flash_died = true; + return; + } + + flash_died = false; + + AP_Logger_Block::Init(); + + //flash_test(); +} + +/* + wait for busy flag to be cleared + */ +void AP_Logger_DataFlash::WaitReady() +{ + if (flash_died) { + return; + } + + uint32_t t = AP_HAL::millis(); + while (Busy()) { + hal.scheduler->delay_microseconds(100); + if (AP_HAL::millis() - t > 5000) { + printf("DataFlash: flash_died\n"); + flash_died = true; + break; + } + } +} + +bool AP_Logger_DataFlash::getSectorCount(void) +{ + WaitReady(); + + WITH_SEMAPHORE(dev_sem); + + // Read manufacturer ID + uint8_t cmd = JEDEC_DEVICE_ID; + dev->transfer(&cmd, 1, buffer, 4); + + uint32_t id = buffer[0] << 16 | buffer[1] << 8 | buffer[2]; + + uint32_t sectors = 0; + + switch (id) { + case JEDEC_ID_WINBOND_W25Q16: + case JEDEC_ID_MICRON_M25P16: + sectors = 32; + df_PagePerSector = 256; + break; + case JEDEC_ID_WINBOND_W25Q32: + case JEDEC_ID_MACRONIX_MX25L3206E: + sectors = 64; + df_PagePerSector = 256; + break; + case JEDEC_ID_MICRON_N25Q064: + case JEDEC_ID_WINBOND_W25Q64: + case JEDEC_ID_MACRONIX_MX25L6406E: + sectors = 128; + df_PagePerSector = 256; + break; + case JEDEC_ID_MICRON_N25Q128: + case JEDEC_ID_WINBOND_W25Q128: + case JEDEC_ID_CYPRESS_S25FL128L: + sectors = 256; + df_PagePerSector = 256; + break; + case JEDEC_ID_WINBOND_W25Q256: + case JEDEC_ID_MACRONIX_MX25L25635E: + sectors = 512; + df_PagePerSector = 256; + use_32bit_address = true; + break; + default: + hal.scheduler->delay(2000); + printf("Unknown SPI Flash 0x%08x\n", id); + return false; + } + + df_PageSize = 256; + df_NumPages = sectors * df_PagePerSector; + erase_cmd = JEDEC_BLOCK64_ERASE; + + hal.scheduler->delay(2000); + printf("SPI Flash 0x%08x found pages=%u erase=%uk\n", + id, df_NumPages, (df_PagePerSector * (uint32_t)df_PageSize)/1024); + return true; + +} + +// Read the status register +uint8_t AP_Logger_DataFlash::ReadStatusReg() +{ + WITH_SEMAPHORE(dev_sem); + uint8_t cmd = JEDEC_READ_STATUS; + uint8_t status; + dev->transfer(&cmd, 1, &status, 1); + return status; +} + +bool AP_Logger_DataFlash::Busy() +{ + int32_t status = ReadStatusReg(); + if (status < 0) { + return true; + } + return (status & JEDEC_STATUS_BUSY) != 0; +} + + +/* + send a command with an address +*/ +void AP_Logger_DataFlash::send_command_addr(uint8_t command, uint32_t PageAdr) +{ + uint8_t cmd[5]; + cmd[0] = command; + if (use_32bit_address) { + cmd[1] = (PageAdr >> 24) & 0xff; + cmd[2] = (PageAdr >> 16) & 0xff; + cmd[3] = (PageAdr >> 8) & 0xff; + cmd[4] = (PageAdr >> 0) & 0xff; + } else { + cmd[1] = (PageAdr >> 16) & 0xff; + cmd[2] = (PageAdr >> 8) & 0xff; + cmd[3] = (PageAdr >> 0) & 0xff; + } + + dev->transfer(cmd, use_32bit_address?5:4, NULL, 0); +} + + +void AP_Logger_DataFlash::PageToBuffer(uint32_t pageNum) +{ + if (pageNum == 0 || pageNum > df_NumPages+1) { + printf("Invalid page read %u\n", pageNum); + memset(buffer, 0xFF, df_PageSize); + return; + } + WaitReady(); + + uint32_t PageAdr = (pageNum-1) * df_PageSize; + + WITH_SEMAPHORE(dev_sem); + dev->set_chip_select(true); + send_command_addr(JEDEC_READ_DATA, PageAdr); + dev->transfer(NULL, 0, buffer, df_PageSize); + dev->set_chip_select(false); +} + +void AP_Logger_DataFlash::BufferToPage(uint32_t pageNum) +{ + if (pageNum == 0 || pageNum > df_NumPages+1) { + printf("Invalid page write %u\n", pageNum); + return; + } + WriteEnable(); + + WITH_SEMAPHORE(dev_sem); + + uint32_t PageAdr = (pageNum-1) * df_PageSize; + + dev->set_chip_select(true); + send_command_addr(JEDEC_PAGE_WRITE, PageAdr); + dev->transfer(buffer, df_PageSize, NULL, 0); + dev->set_chip_select(false); +} + +/* + erase one sector (sizes varies with hw) +*/ +void AP_Logger_DataFlash::SectorErase(uint32_t sectorNum) +{ + WriteEnable(); + + WITH_SEMAPHORE(dev_sem); + + uint32_t PageAdr = sectorNum * df_PageSize * df_PagePerSector; + send_command_addr(erase_cmd, PageAdr); +} + + +void AP_Logger_DataFlash::StartErase() +{ + WriteEnable(); + + WITH_SEMAPHORE(dev_sem); + + uint8_t cmd = JEDEC_BULK_ERASE; + dev->transfer(&cmd, 1, NULL, 0); + + erase_start_ms = AP_HAL::millis(); + printf("Dataflash: erase started\n"); +} + +bool AP_Logger_DataFlash::InErase() +{ + if (erase_start_ms && !Busy()) { + printf("Dataflash: erase done (%u ms)\n", AP_HAL::millis() - erase_start_ms); + erase_start_ms = 0; + } + return erase_start_ms != 0; +} + + +void AP_Logger_DataFlash::WriteEnable(void) +{ + WaitReady(); + WITH_SEMAPHORE(dev_sem); + uint8_t b = JEDEC_WRITE_ENABLE; + dev->transfer(&b, 1, NULL, 0); +} + +void AP_Logger_DataFlash::flash_test() +{ + for (uint8_t i=1; i<=20; i++) { + printf("Flash fill %u\n", i); + if (i % df_PagePerSector == 0) { + SectorErase(i / df_PagePerSector); + } + memset(buffer, i, df_PageSize); + BufferToPage(i); + } + for (uint8_t i=1; i<=20; i++) { + printf("Flash check %u\n", i); + PageToBuffer(i); + for (uint16_t j=0; j + +#ifdef HAL_LOGGING_DATAFLASH +#include "AP_Logger_Backend.h" +#include "AP_Logger_Block.h" + + +class AP_Logger_DataFlash : public AP_Logger_Block { +public: + AP_Logger_DataFlash(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) : + AP_Logger_Block(front, writer) {} + void Init(void) override; + bool CardInserted() const override { return !flash_died && df_NumPages > 0; } + +private: + void BufferToPage(uint32_t PageAdr) override; + void PageToBuffer(uint32_t PageAdr) override; + void SectorErase(uint32_t SectorAdr) override; + void StartErase() override; + bool InErase() override; + void send_command_addr(uint8_t cmd, uint32_t address); + void WaitReady(); + bool Busy(); + uint8_t ReadStatusReg(); + + void WriteEnable(); + bool getSectorCount(void); + void flash_test(void); + + AP_HAL::OwnPtr dev; + AP_HAL::Semaphore *dev_sem; + + bool flash_died; + uint32_t erase_start_ms; + uint8_t erase_cmd; + bool use_32bit_address; +}; + +#endif // HAL_LOGGING_DATAFLASH diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index bc4969b276..c5f23337fd 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -655,7 +655,7 @@ uint16_t AP_Logger_File::_log_num_from_list_entry(const uint16_t list_entry) /* find the number of pages in a log */ -void AP_Logger_File::get_log_boundaries(const uint16_t list_entry, uint16_t & start_page, uint16_t & end_page) +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); if (log_num == 0) { diff --git a/libraries/AP_Logger/AP_Logger_File.h b/libraries/AP_Logger/AP_Logger_File.h index 74ccc8282c..4d8cb66502 100644 --- a/libraries/AP_Logger/AP_Logger_File.h +++ b/libraries/AP_Logger/AP_Logger_File.h @@ -36,7 +36,7 @@ public: // high level interface uint16_t find_last_log() override; - void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) override; + void get_log_boundaries(uint16_t log_num, 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; 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; diff --git a/libraries/AP_Logger/AP_Logger_MAVLink.h b/libraries/AP_Logger/AP_Logger_MAVLink.h index 81e44750db..885a606db0 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLink.h +++ b/libraries/AP_Logger/AP_Logger_MAVLink.h @@ -54,7 +54,7 @@ public: // 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_boundaries(uint16_t log_num, 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 {} 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; } diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index ff132111a5..e0b35835e0 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -131,7 +131,7 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *ms _log_num_data = packet.id; _log_data_size = size; - uint16_t end; + uint32_t end; get_log_boundaries(packet.id, _log_data_page, end); } diff --git a/libraries/AP_Logger/AP_Logger_Revo.cpp b/libraries/AP_Logger/AP_Logger_Revo.cpp index d617ccfe15..15d4d9b001 100644 --- a/libraries/AP_Logger/AP_Logger_Revo.cpp +++ b/libraries/AP_Logger/AP_Logger_Revo.cpp @@ -59,7 +59,7 @@ void AP_Logger_Revo::StartWrite(uint16_t PageAdr) void AP_Logger_Revo::FinishWrite(void) { // Write Buffer to flash, NO WAIT - BufferToPage(df_BufferNum, df_PageAdr, 0); + BufferToPage(df_BufferNum, df_PageAdr); df_PageAdr++; // If we reach the end of the memory, start from the beginning if (df_PageAdr > df_NumPages) { @@ -596,7 +596,7 @@ void AP_Logger_Revo::PageToBuffer(unsigned char BufferNum, uint16_t pageNum) cs_release(); } -void AP_Logger_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum, unsigned char wait) +void AP_Logger_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum) { uint32_t PageAdr = pageNum * DF_PAGE_SIZE; @@ -614,9 +614,6 @@ void AP_Logger_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum, un _spi->transfer(buffer[BufferNum], DF_PAGE_SIZE, NULL, 0); cs_release(); - - if(wait) WaitReady(); - } void AP_Logger_Revo::BufferWrite (unsigned char BufferNum, uint16_t IntPageAdr, unsigned char Data) diff --git a/libraries/AP_Logger/AP_Logger_Revo.h b/libraries/AP_Logger/AP_Logger_Revo.h index 05e9ab4843..b3ea204148 100644 --- a/libraries/AP_Logger/AP_Logger_Revo.h +++ b/libraries/AP_Logger/AP_Logger_Revo.h @@ -48,13 +48,12 @@ class AP_Logger_Revo : public AP_Logger_Backend private: //Methods void BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data); - void BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait); + void BufferToPage (uint8_t BufferNum, uint16_t PageAdr); void PageToBuffer(uint8_t BufferNum, uint16_t PageAdr); void WaitReady(); uint8_t ReadStatusReg(); uint16_t PageSize() { return df_PageSize; } void PageErase (uint16_t PageAdr); - void BlockErase (uint16_t BlockAdr); void ChipErase(); void Flash_Jedec_WriteEnable(); diff --git a/libraries/AP_Logger/AP_Logger_SITL.cpp b/libraries/AP_Logger/AP_Logger_SITL.cpp new file mode 100644 index 0000000000..5791b063f5 --- /dev/null +++ b/libraries/AP_Logger/AP_Logger_SITL.cpp @@ -0,0 +1,99 @@ +/* + backend for SITL emulation of block logging +*/ + +#include "AP_Logger_SITL.h" + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include +#include +#include +#include +#include +#include + +#define DF_PAGE_SIZE 256UL +#define DF_PAGE_PER_SECTOR 16 // 4k sectors +#define DF_NUM_PAGES 65536UL + +#define ERASE_TIME_MS 10000 + +extern const AP_HAL::HAL& hal; + +void AP_Logger_SITL::Init() +{ + if (flash_fd == 0) { + flash_fd = open("dataflash.bin", O_RDWR|O_CLOEXEC, 0777); + if (flash_fd == -1) { + flash_fd = open("dataflash.bin", 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 dataflash.bin"); + } + } + + df_PageSize = DF_PAGE_SIZE; + df_PagePerSector = DF_PAGE_PER_SECTOR; + df_NumPages = DF_NUM_PAGES; + + AP_Logger_Block::Init(); +} + +bool AP_Logger_SITL::CardInserted(void) const +{ + return df_NumPages > 0; +} + +void AP_Logger_SITL::PageToBuffer(uint32_t PageAdr) +{ + assert(PageAdr>0 && PageAdr <= df_NumPages+1); + if (pread(flash_fd, buffer, DF_PAGE_SIZE, (PageAdr-1)*DF_PAGE_SIZE) != DF_PAGE_SIZE) { + printf("Failed flash read"); + } +} + +void AP_Logger_SITL::BufferToPage(uint32_t PageAdr) +{ + assert(PageAdr>0 && PageAdr <= df_NumPages+1); + if (pwrite(flash_fd, buffer, DF_PAGE_SIZE, (PageAdr-1)*DF_PAGE_SIZE) != DF_PAGE_SIZE) { + printf("Failed flash write"); + } +} + +void AP_Logger_SITL::SectorErase(uint32_t 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 erase"); + } +} + +void AP_Logger_SITL::StartErase() +{ + for (uint32_t i=0; i + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include "AP_Logger_Block.h" + +class AP_Logger_SITL : public AP_Logger_Block { +public: + AP_Logger_SITL(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) : + AP_Logger_Block(front, writer) {} + void Init() override; + bool CardInserted() const override; + +private: + void BufferToPage(uint32_t PageAdr) override; + void PageToBuffer(uint32_t PageAdr) override; + void SectorErase(uint32_t SectorAdr) override; + void StartErase() override; + bool InErase() override; + + int flash_fd; + uint32_t erase_started_ms; +}; + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL