From 5e4f66e525b0c0930071b1f25c798e2aa47a1290 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Dec 2011 15:52:36 +1100 Subject: [PATCH] DataFlash: moved high level logging logic to library this moves out all the high level logic from ArduPlane/ArduCopter to the library. It also adds a "config page", as the last page in the flash. This is used to check if the flash needs erasing. We only erase now if the DF_LOGGING_FORMAT has changed. This patch also adds a public CardInserted() method, which is used to disable logging on APM2 if a dataflash card is not inserted --- libraries/DataFlash/DataFlash.cpp | 243 +++++++++++++++++++ libraries/DataFlash/DataFlash.h | 33 ++- libraries/DataFlash/DataFlash_APM1.cpp | 14 +- libraries/DataFlash/DataFlash_APM1.h | 5 +- libraries/DataFlash/DataFlash_APM2.cpp | 8 +- libraries/DataFlash/DataFlash_APM2.h | 5 +- libraries/Desktop/support/DataFlash_APM1.cpp | 12 +- 7 files changed, 301 insertions(+), 19 deletions(-) diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 6aa796c263..5e192ea041 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -185,3 +185,246 @@ uint16_t DataFlash_Class::GetFilePage() { return df_FilePage; } + +void DataFlash_Class::EraseAll(void (*delay_cb)(unsigned long)) +{ + SetFileNumber(0xFFFF); + for(uint16_t j = 1; j <= df_NumPages; j++) { + PageErase(j); + StartWrite(j); + delay_cb(1); + } + + // write the logging format in the last page + StartWrite(df_NumPages+1); + WriteLong(DF_LOGGING_FORMAT); + FinishWrite(); +} + +/* + we need to erase if the logging format has changed + */ +bool DataFlash_Class::NeedErase(void) +{ + StartRead(df_NumPages+1); + return ReadLong() != DF_LOGGING_FORMAT; +} + + + +// This function determines the number of whole or partial log files in the DataFlash +// Wholly overwritten files are (of course) lost. +uint8_t DataFlash_Class::get_num_logs(void) +{ + uint16_t lastpage; + uint16_t last; + uint16_t first; + + if(find_last_page() == 1) return 0; + + StartRead(1); + + if(GetFileNumber() == 0XFFFF) return 0; + + lastpage = find_last_page(); + StartRead(lastpage); + last = GetFileNumber(); + StartRead(lastpage + 2); + first = GetFileNumber(); + if(first > last) { + StartRead(1); + first = GetFileNumber(); + } + if(last == first) + { + return 1; + } else { + return (last - first + 1); + } +} + +// This function starts a new log file in the DataFlash +void DataFlash_Class::start_new_log(void) +{ + uint16_t last_page = find_last_page(); + + StartRead(last_page); + //Serial.print("last page: "); Serial.println(last_page); + //Serial.print("file #: "); Serial.println(GetFileNumber()); + //Serial.print("file page: "); Serial.println(GetFilePage()); + + if(find_last_log() == 0 || GetFileNumber() == 0xFFFF) { + SetFileNumber(1); + StartWrite(1); + //Serial.println("start log from 0"); + return; + } + + // Check for log of length 1 page and suppress + if(GetFilePage() <= 1) { + SetFileNumber(GetFileNumber()); // Last log too short, reuse its number + StartWrite(last_page); // and overwrite it + //Serial.println("start log from short"); + } else { + if(last_page == 0xFFFF) last_page=0; + SetFileNumber(GetFileNumber()+1); + StartWrite(last_page + 1); + //Serial.println("start log normal"); + } +} + +// This function finds the first and last pages of a log file +// The first page may be greater than the last page if the DataFlash has been filled and partially overwritten. +void DataFlash_Class::get_log_boundaries(uint8_t log_num, int & start_page, int & end_page) +{ + int num = get_num_logs(); + int 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 == (int)df_NumPages+1 || start_page == 0) start_page=1; + end_page = find_last_page_of_log((uint16_t)log_num); + if(end_page <= 0) end_page = start_page; +} + +bool DataFlash_Class::check_wrapped(void) +{ + StartRead(df_NumPages); + if(GetFileNumber() == 0xFFFF) + return 0; + else + return 1; +} + + +// This funciton finds the last log number +int DataFlash_Class::find_last_log(void) +{ + int last_page = find_last_page(); + StartRead(last_page); + return GetFileNumber(); +} + +// This function finds the last page of the last file +int DataFlash_Class::find_last_page(void) +{ + uint16_t look; + uint16_t bottom = 1; + uint16_t top = df_NumPages; + uint32_t look_hash; + uint32_t bottom_hash; + uint32_t top_hash; + + StartRead(bottom); + bottom_hash = ((long)GetFileNumber()<<16) | GetFilePage(); + + while(top-bottom > 1) { + look = (top+bottom)/2; + StartRead(look); + look_hash = (long)GetFileNumber()<<16 | GetFilePage(); + 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 = ((long)GetFileNumber()<<16) | GetFilePage(); + 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 +int DataFlash_Class::find_last_page_of_log(uint16_t log_number) +{ + uint16_t look; + uint16_t bottom; + uint16_t top; + uint32_t look_hash; + uint32_t check_hash; + + 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 = (long)log_number<<16 | 0xFFFF; + + while(top-bottom > 1) + { + look = (top+bottom)/2; + StartRead(look); + look_hash = (long)GetFileNumber()<<16 | GetFilePage(); + 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 -1; +} diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index d9b5c3bda1..c4af0eb567 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -8,6 +8,10 @@ #define DF_OVERWRITE_DATA 1 // 0: When reach the end page stop, 1: Start overwriting from page 1 +// 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 0x28122011 + class DataFlash_Class { private: @@ -27,20 +31,31 @@ class DataFlash_Class virtual void BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait) = 0; virtual void PageToBuffer(unsigned char BufferNum, uint16_t PageAdr) = 0; virtual unsigned char BufferRead (unsigned char BufferNum, uint16_t IntPageAdr) = 0; + virtual void PageErase(uint16_t PageAdr) = 0; + virtual void ChipErase(void) = 0; + + // internal high level functions + int find_last_page(void); + int find_last_page_of_log(uint16_t log_number); + bool check_wrapped(void); public: unsigned char df_manufacturer; - unsigned char df_device_0; - unsigned char df_device_1; + uint16_t df_device; + uint16_t df_PageSize; DataFlash_Class() {} // Constructor virtual void Init(void) = 0; virtual void ReadManufacturerID() = 0; + virtual bool CardInserted(void) = 0; + int16_t GetPage(void); int16_t GetWritePage(void); - virtual void PageErase(uint16_t PageAdr) = 0; - virtual void ChipErase(void) = 0; + + // erase handling + void EraseAll(void (*delay_cb)(unsigned long)); + bool NeedErase(void); // Write methods void StartWrite(int16_t PageAdr); @@ -55,12 +70,20 @@ class DataFlash_Class int16_t ReadInt(); int32_t ReadLong(); + // file numbers void SetFileNumber(uint16_t FileNumber); uint16_t GetFileNumber(); uint16_t GetFilePage(); - uint16_t df_PageSize; + // page handling uint16_t df_NumPages; + + // high level interface + int find_last_log(void); + void get_log_boundaries(uint8_t log_num, int & start_page, int & end_page); + uint8_t get_num_logs(void); + void start_new_log(void); + }; #include "DataFlash_APM1.h" diff --git a/libraries/DataFlash/DataFlash_APM1.cpp b/libraries/DataFlash/DataFlash_APM1.cpp index 66e32ac8a1..9057c8b8f2 100644 --- a/libraries/DataFlash/DataFlash_APM1.cpp +++ b/libraries/DataFlash/DataFlash_APM1.cpp @@ -115,7 +115,9 @@ void DataFlash_APM1::Init(void) // get page size: 512 or 528 df_PageSize=PageSize(); - df_NumPages = DF_LAST_PAGE; + + // the last page is reserved for config information + df_NumPages = DF_LAST_PAGE - 1; } // This function is mainly to test the device @@ -127,13 +129,19 @@ void DataFlash_APM1::ReadManufacturerID() SPI.transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID); df_manufacturer = SPI.transfer(0xff); - df_device_0 = SPI.transfer(0xff); - df_device_1 = SPI.transfer(0xff); + df_device = SPI.transfer(0xff); + df_device = (df_device<<8) | SPI.transfer(0xff); SPI.transfer(0xff); dataflash_CS_inactive(); // Reset dataflash command decoder } + +bool DataFlash_APM1::CardInserted(void) +{ + return true; +} + // Read the status register byte DataFlash_APM1::ReadStatusReg() { diff --git a/libraries/DataFlash/DataFlash_APM1.h b/libraries/DataFlash/DataFlash_APM1.h index 00cf16a7b8..9761c08ada 100644 --- a/libraries/DataFlash/DataFlash_APM1.h +++ b/libraries/DataFlash/DataFlash_APM1.h @@ -18,14 +18,15 @@ class DataFlash_APM1 : public DataFlash_Class unsigned char ReadStatusReg(); unsigned char ReadStatus(); uint16_t PageSize(); + void PageErase (uint16_t PageAdr); + void ChipErase (); public: DataFlash_APM1(); // Constructor void Init(); void ReadManufacturerID(); - void PageErase (uint16_t PageAdr); - void ChipErase (); + bool CardInserted(void); }; #endif // __DATAFLASH_APM1_H__ diff --git a/libraries/DataFlash/DataFlash_APM2.cpp b/libraries/DataFlash/DataFlash_APM2.cpp index 8fd8b5eb38..415cdc062d 100644 --- a/libraries/DataFlash/DataFlash_APM2.cpp +++ b/libraries/DataFlash/DataFlash_APM2.cpp @@ -132,7 +132,9 @@ void DataFlash_APM2::Init(void) // get page size: 512 or 528 (by default: 528) df_PageSize=PageSize(); - df_NumPages = DF_LAST_PAGE; + + // the last page is reserved for config information + df_NumPages = DF_LAST_PAGE - 1; } // This function is mainly to test the device @@ -145,8 +147,8 @@ void DataFlash_APM2::ReadManufacturerID() SPI_transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID); df_manufacturer = SPI_transfer(0xff); - df_device_0 = SPI_transfer(0xff); - df_device_1 = SPI_transfer(0xff); + df_device = SPI_transfer(0xff); + df_device = (df_device<<8) | SPI_transfer(0xff); SPI_transfer(0xff); } diff --git a/libraries/DataFlash/DataFlash_APM2.h b/libraries/DataFlash/DataFlash_APM2.h index 20cd368afd..73a3ce5fce 100644 --- a/libraries/DataFlash/DataFlash_APM2.h +++ b/libraries/DataFlash/DataFlash_APM2.h @@ -22,15 +22,14 @@ class DataFlash_APM2 : public DataFlash_Class unsigned char SPI_transfer(unsigned char data); void CS_inactive(); void CS_active(); - + void PageErase (uint16_t PageAdr); + void ChipErase (); public: DataFlash_APM2(); // Constructor void Init(); void ReadManufacturerID(); bool CardInserted(); - void PageErase (uint16_t PageAdr); - void ChipErase (); }; #endif diff --git a/libraries/Desktop/support/DataFlash_APM1.cpp b/libraries/Desktop/support/DataFlash_APM1.cpp index 289a845bef..d2e00f09d3 100644 --- a/libraries/Desktop/support/DataFlash_APM1.cpp +++ b/libraries/Desktop/support/DataFlash_APM1.cpp @@ -32,15 +32,21 @@ void DataFlash_APM1::Init(void) } } df_PageSize = DF_PAGE_SIZE; - df_NumPages = DF_NUM_PAGES; + + // reserve last page for config information + df_NumPages = DF_NUM_PAGES - 1; } // This function is mainly to test the device void DataFlash_APM1::ReadManufacturerID() { df_manufacturer = 1; - df_device_0 = 2; - df_device_1 = 3; + df_device = 0x0203; +} + +bool DataFlash_APM1::CardInserted(void) +{ + return true; } // Read the status register