diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 103cb30c70..02035ddc16 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -128,6 +128,11 @@ void DataFlash_Class::ReadBlock(void *pBuffer, uint16_t size) } } +void DataFlash_Class::ReadPacket(void *pkt, uint16_t size) +{ + ReadBlock((void *)(sizeof(struct log_Header)+(uintptr_t)pkt), size - sizeof(struct log_Header)); +} + void DataFlash_Class::SetFileNumber(uint16_t FileNumber) { df_FileNumber = FileNumber; @@ -170,3 +175,4 @@ bool DataFlash_Class::NeedErase(void) return version != DF_LOGGING_FORMAT; } + diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 4863a98418..5492e64586 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -17,6 +17,40 @@ class DataFlash_Class { +public: + // initialisation + virtual void Init(void) = 0; + virtual bool CardInserted(void) = 0; + + // erase handling + bool NeedErase(void); + void EraseAll(); + + /* Write a block of data at current offset */ + void WriteBlock(const void *pBuffer, uint16_t size); + + /* + read a packet, stripping off the header bytes + */ + void ReadPacket(void *pkt, uint16_t size); + + // high level interface + int16_t find_last_log(void); + void get_log_boundaries(uint8_t log_num, int16_t & start_page, int16_t & end_page); + uint8_t get_num_logs(void); + void start_new_log(void); + uint16_t log_read_process(uint16_t start_page, uint16_t end_page, + void (*callback)(uint8_t msgid)); + void DumpPageInfo(AP_HAL::BetterStream *port); + void ShowDeviceInfo(AP_HAL::BetterStream *port); + + /* + every logged packet starts with 3 bytes + */ + struct log_Header { + uint8_t head1, head2, msgid; + }; + private: struct PageHeader { uint16_t FileNumber; @@ -52,67 +86,35 @@ private: // start of the page virtual bool BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size) = 0; + // start reading at the given page + void StartRead(int16_t PageAdr); + // internal high level functions int16_t find_last_page(void); int16_t find_last_page_of_log(uint16_t log_number); bool check_wrapped(void); - -public: - uint8_t df_manufacturer; - uint16_t df_device; - uint16_t df_PageSize; - - virtual void Init(void) = 0; - virtual void ReadManufacturerID() = 0; - virtual bool CardInserted(void) = 0; - int16_t GetPage(void); int16_t GetWritePage(void); - - // erase handling - void EraseAll(); - bool NeedErase(void); - - // Write methods void StartWrite(int16_t PageAdr); void FinishWrite(void); - void WriteBlock(const void *pBuffer, uint16_t size); - // Read methods - void StartRead(int16_t PageAdr); void ReadBlock(void *pBuffer, uint16_t size); // file numbers void SetFileNumber(uint16_t FileNumber); - uint16_t GetFileNumber(); uint16_t GetFilePage(); + uint16_t GetFileNumber(); + +protected: + uint8_t df_manufacturer; + uint16_t df_device; // page handling + uint16_t df_PageSize; uint16_t df_NumPages; - /* - read a packet, stripping off the header bytes - */ - void ReadPacket(void *pkt, uint16_t size) { - ReadBlock((void *)(sizeof(struct log_Header)+(uintptr_t)pkt), size - sizeof(struct log_Header)); - } - - // high level interface - int16_t find_last_log(void); - void get_log_boundaries(uint8_t log_num, int16_t & start_page, int16_t & end_page); - uint8_t get_num_logs(void); - void start_new_log(void); - uint16_t log_read_process(uint16_t start_page, uint16_t end_page, - void (*callback)(uint8_t msgid)); - void DumpPageInfo(AP_HAL::BetterStream *port); - - /* - every logged packet starts with 3 bytes - */ - struct log_Header { - uint8_t head1, head2, msgid; - }; + virtual void ReadManufacturerID() = 0; }; /* @@ -128,7 +130,6 @@ public: #define HEAD_BYTE2 0x95 // Decimal 149 - #include "DataFlash_APM1.h" #include "DataFlash_APM2.h" #include "DataFlash_SITL.h" diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index d9e066e6b1..2fc1ce57e9 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -226,9 +226,11 @@ int16_t DataFlash_Class::find_last_page_of_log(uint16_t log_number) } -// Read the DataFlash log memory : Packet Parser. Call the callback() -// function on each log message found in the page range. Return the -// number of log messages found +/* + Read the DataFlash log memory + Call the callback() function on each log message found in the page + range. Return the number of log messages found +*/ uint16_t DataFlash_Class::log_read_process(uint16_t start_page, uint16_t end_page, void (*callback)(uint8_t msgid)) { @@ -266,7 +268,7 @@ uint16_t DataFlash_Class::log_read_process(uint16_t start_page, uint16_t end_pag } uint16_t new_page = GetPage(); if (new_page != page) { - if (new_page == end_page) { + if (new_page == end_page || new_page == start_page) { return packet_count; } page = new_page; @@ -289,3 +291,21 @@ void DataFlash_Class::DumpPageInfo(AP_HAL::BetterStream *port) } } +/* + show information about the device + */ +void DataFlash_Class::ShowDeviceInfo(AP_HAL::BetterStream *port) +{ + if (!CardInserted()) { + port->println_P(PSTR("No dataflash inserted")); + return; + } + ReadManufacturerID(); + port->printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), + (unsigned)df_manufacturer, + (unsigned)df_device); + port->printf_P(PSTR("NumPages: %u PageSize: %u\n"), + (unsigned)df_NumPages+1, + (unsigned)df_PageSize); +} +