diff --git a/libraries/AP_Logger/AP_Logger_Block.cpp b/libraries/AP_Logger/AP_Logger_Block.cpp index 4404aaaee6..e261fe6df5 100644 --- a/libraries/AP_Logger/AP_Logger_Block.cpp +++ b/libraries/AP_Logger/AP_Logger_Block.cpp @@ -183,13 +183,12 @@ bool AP_Logger_Block::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, // read from the page address and return the file number at that location uint16_t AP_Logger_Block::StartRead(uint32_t PageAdr) { - df_Read_PageAdr = PageAdr; - // copy flash page to buffer if (erase_started) { + df_Read_PageAdr = PageAdr; memset(buffer, 0xff, df_PageSize); } else { - PageToBuffer(df_Read_PageAdr); + PageToBuffer(PageAdr); } return ReadHeaders(); } @@ -239,14 +238,15 @@ bool AP_Logger_Block::ReadBlock(void *pBuffer, uint16_t size) df_Read_BufferIdx += n; if (df_Read_BufferIdx == df_PageSize) { - df_Read_PageAdr++; - if (df_Read_PageAdr > df_NumPages) { - df_Read_PageAdr = 1; + uint32_t new_page_addr = df_Read_PageAdr + 1; + if (new_page_addr > df_NumPages) { + new_page_addr = 1; } if (erase_started) { memset(buffer, 0xff, df_PageSize); + df_Read_PageAdr = new_page_addr; } else { - PageToBuffer(df_Read_PageAdr); + PageToBuffer(new_page_addr); } // We are starting a new page - read FileNumber and FilePage diff --git a/libraries/AP_Logger/AP_Logger_Block.h b/libraries/AP_Logger/AP_Logger_Block.h index 24a58ad12e..8061137a57 100644 --- a/libraries/AP_Logger/AP_Logger_Block.h +++ b/libraries/AP_Logger/AP_Logger_Block.h @@ -62,6 +62,7 @@ protected: uint8_t *buffer; uint32_t last_messagewrite_message_sent; + uint32_t df_Read_PageAdr; private: /* @@ -95,8 +96,7 @@ private: // state variables uint16_t df_Read_BufferIdx; - uint32_t df_PageAdr; - uint32_t df_Read_PageAdr; + uint32_t df_PageAdr; // current page address for writes // file numbers uint16_t df_FileNumber; uint16_t df_Write_FileNumber; diff --git a/libraries/AP_Logger/AP_Logger_DataFlash.cpp b/libraries/AP_Logger/AP_Logger_DataFlash.cpp index 02f6db3250..b846f63cd9 100644 --- a/libraries/AP_Logger/AP_Logger_DataFlash.cpp +++ b/libraries/AP_Logger/AP_Logger_DataFlash.cpp @@ -219,8 +219,17 @@ 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); + df_Read_PageAdr = pageNum; return; } + + // we already just read this page + if (pageNum == df_Read_PageAdr && read_cache_valid) { + return; + } + + df_Read_PageAdr = pageNum; + WaitReady(); uint32_t PageAdr = (pageNum-1) * df_PageSize; @@ -230,6 +239,8 @@ void AP_Logger_DataFlash::PageToBuffer(uint32_t pageNum) send_command_addr(JEDEC_READ_DATA, PageAdr); dev->transfer(nullptr, 0, buffer, df_PageSize); dev->set_chip_select(false); + + read_cache_valid = true; } void AP_Logger_DataFlash::BufferToPage(uint32_t pageNum) @@ -238,6 +249,12 @@ void AP_Logger_DataFlash::BufferToPage(uint32_t pageNum) printf("Invalid page write %u\n", pageNum); return; } + + // about to write the cached page + if (pageNum != df_Read_PageAdr) { + read_cache_valid = false; + } + WriteEnable(); WITH_SEMAPHORE(dev_sem); diff --git a/libraries/AP_Logger/AP_Logger_DataFlash.h b/libraries/AP_Logger/AP_Logger_DataFlash.h index 813e954901..6bc933ec93 100644 --- a/libraries/AP_Logger/AP_Logger_DataFlash.h +++ b/libraries/AP_Logger/AP_Logger_DataFlash.h @@ -43,6 +43,7 @@ private: uint32_t erase_start_ms; uint8_t erase_cmd; bool use_32bit_address; + bool read_cache_valid; }; #endif // HAL_LOGGING_DATAFLASH_ENABLED diff --git a/libraries/AP_Logger/AP_Logger_W25N01GV.cpp b/libraries/AP_Logger/AP_Logger_W25N01GV.cpp index f4435835bc..86ce642c99 100644 --- a/libraries/AP_Logger/AP_Logger_W25N01GV.cpp +++ b/libraries/AP_Logger/AP_Logger_W25N01GV.cpp @@ -198,9 +198,17 @@ void AP_Logger_W25N01GV::PageToBuffer(uint32_t pageNum) if (pageNum == 0 || pageNum > df_NumPages+1) { printf("Invalid page read %u\n", pageNum); memset(buffer, 0xFF, df_PageSize); + df_Read_PageAdr = pageNum; return; } + // we already just read this page + if (pageNum == df_Read_PageAdr && read_cache_valid) { + return; + } + + df_Read_PageAdr = pageNum; + WaitReady(); uint32_t PageAdr = (pageNum-1); @@ -224,6 +232,8 @@ void AP_Logger_W25N01GV::PageToBuffer(uint32_t pageNum) dev->transfer(cmd, 4, nullptr, 0); dev->transfer(nullptr, 0, buffer, df_PageSize); dev->set_chip_select(false); + + read_cache_valid = true; } } @@ -234,6 +244,11 @@ void AP_Logger_W25N01GV::BufferToPage(uint32_t pageNum) return; } + // just wrote the cached page + if (pageNum != df_Read_PageAdr) { + read_cache_valid = false; + } + WriteEnable(); uint32_t PageAdr = (pageNum-1); diff --git a/libraries/AP_Logger/AP_Logger_W25N01GV.h b/libraries/AP_Logger/AP_Logger_W25N01GV.h index 0865d355ed..556603eaff 100644 --- a/libraries/AP_Logger/AP_Logger_W25N01GV.h +++ b/libraries/AP_Logger/AP_Logger_W25N01GV.h @@ -42,6 +42,7 @@ private: bool flash_died; uint32_t erase_start_ms; uint16_t erase_block; + bool read_cache_valid; }; #endif // HAL_LOGGING_DATAFLASH_ENABLED