From 62f190ed158febb44ce45c10f3c502fb7523a67b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Apr 2013 10:25:10 +1000 Subject: [PATCH] DataFlash: simplify code now that copter is converted no longer need as many public functions --- libraries/DataFlash/DataFlash.h | 16 +++--- libraries/DataFlash/DataFlash_Block.cpp | 5 -- libraries/DataFlash/DataFlash_Block.h | 8 --- libraries/DataFlash/DataFlash_File.cpp | 72 +------------------------ libraries/DataFlash/DataFlash_File.h | 13 ++--- libraries/DataFlash/LogFile.cpp | 59 ++------------------ 6 files changed, 16 insertions(+), 157 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 1aed99813b..3a391781e0 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -34,19 +34,10 @@ public: /* Write a block of data at current offset */ virtual void WriteBlock(const void *pBuffer, uint16_t size) = 0; - /* - read a packet. The header byte have already been read. - */ - virtual void ReadPacket(void *pkt, uint16_t size) = 0; - // high level interface virtual uint16_t find_last_log(void) = 0; virtual void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) = 0; virtual uint16_t get_num_logs(void) = 0; - virtual uint16_t start_new_log(void) = 0; - void log_read_process(uint16_t log_num, - uint16_t start_page, uint16_t end_page, - void (*callback)(uint8_t msgid)); virtual void LogReadProcess(uint16_t log_num, uint16_t start_page, uint16_t end_page, uint8_t num_types, @@ -90,6 +81,13 @@ protected: void Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token, enum ap_var_type type); void Log_Write_Parameters(void); + virtual uint16_t start_new_log(void) = 0; + + /* + read a block + */ + virtual void ReadBlock(void *pkt, uint16_t size) = 0; + }; /* diff --git a/libraries/DataFlash/DataFlash_Block.cpp b/libraries/DataFlash/DataFlash_Block.cpp index b2746ca9fa..4f31a34542 100644 --- a/libraries/DataFlash/DataFlash_Block.cpp +++ b/libraries/DataFlash/DataFlash_Block.cpp @@ -131,11 +131,6 @@ void DataFlash_Block::ReadBlock(void *pBuffer, uint16_t size) } } -void DataFlash_Block::ReadPacket(void *pkt, uint16_t size) -{ - ReadBlock((void *)(sizeof(struct log_Header)+(uintptr_t)pkt), size - sizeof(struct log_Header)); -} - void DataFlash_Block::SetFileNumber(uint16_t FileNumber) { df_FileNumber = FileNumber; diff --git a/libraries/DataFlash/DataFlash_Block.h b/libraries/DataFlash/DataFlash_Block.h index a66ccde405..f2d3dcebfc 100644 --- a/libraries/DataFlash/DataFlash_Block.h +++ b/libraries/DataFlash/DataFlash_Block.h @@ -30,19 +30,11 @@ public: /* Write a block of data at current offset */ void WriteBlock(const void *pBuffer, uint16_t size); - /* - read a packet. The header byte have already been read. - */ - void ReadPacket(void *pkt, uint16_t size); - // high level interface uint16_t find_last_log(void); void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page); uint16_t get_num_logs(void); uint16_t start_new_log(void); - void log_read_process(uint16_t log_num, - uint16_t start_page, uint16_t end_page, - void (*callback)(uint8_t msgid)); void LogReadProcess(uint16_t log_num, uint16_t start_page, uint16_t end_page, uint8_t num_types, diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index e08aa05edf..f6585531dd 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -172,16 +172,12 @@ void DataFlash_File::WriteBlock(const void *pBuffer, uint16_t size) /* read a packet. The header bytes have already been read. */ -void DataFlash_File::ReadPacket(void *pkt, uint16_t size) +void DataFlash_File::ReadBlock(void *pkt, uint16_t size) { if (_read_fd == -1 || !_initialised || size <= 3) { return; } - // we don't read the 3 header bytes. That happens in log_read_process() - pkt = (void *)(3+(char *)pkt); - size -= 3; - memset(pkt, 0, size); ::read(_read_fd, pkt, size); _read_offset += size; @@ -362,72 +358,6 @@ void DataFlash_File::LogReadProcess(uint16_t log_num, _read_fd = -1; } -/* - start processing a log, called by CLI to display logs - */ -void DataFlash_File::log_read_process(uint16_t log_num, - uint16_t start_page, uint16_t end_page, - void (*callback)(uint8_t msgid)) -{ - uint8_t log_step = 0; - if (!_initialised) { - return; - } - if (_read_fd != -1) { - ::close(_read_fd); - } - char *fname = _log_file_name(log_num); - if (fname == NULL) { - return; - } - _read_fd = ::open(fname, O_RDONLY); - free(fname); - if (_read_fd == -1) { - return; - } - _read_offset = 0; - if (start_page != 0) { - ::lseek(_read_fd, start_page * DATAFLASH_PAGE_SIZE, SEEK_SET); - } - - while (true) { - uint8_t data; - if (::read(_read_fd, &data, 1) != 1) { - // reached end of file - break; - } - _read_offset++; - - // This is a state machine to read the packets - switch(log_step) { - case 0: - if (data == HEAD_BYTE1) { - log_step++; - } - break; - - case 1: - if (data == HEAD_BYTE2) { - log_step++; - } else { - log_step = 0; - } - break; - - case 2: - log_step = 0; - callback(data); - break; - } - if (_read_offset >= (end_page+1) * DATAFLASH_PAGE_SIZE) { - break; - } - } - - ::close(_read_fd); - _read_fd = -1; -} - /* this is a lot less verbose than the block interface. Dumping 2Gbyte of logs a page at a time isn't so useful. Just pull the SD card out diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index 986931e7ee..fbfeb223b8 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -27,19 +27,11 @@ public: /* Write a block of data at current offset */ void WriteBlock(const void *pBuffer, uint16_t size); - /* - read a packet. The header byte have already been read. - */ - void ReadPacket(void *pkt, uint16_t size); - // high level interface uint16_t find_last_log(void); void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page); uint16_t get_num_logs(void); uint16_t start_new_log(void); - void log_read_process(uint16_t log_num, - uint16_t start_page, uint16_t end_page, - void (*callback)(uint8_t msgid)); void LogReadProcess(uint16_t log_num, uint16_t start_page, uint16_t end_page, uint8_t num_types, @@ -56,6 +48,11 @@ private: static volatile bool _initialised; const char *_log_directory; + /* + read a block + */ + void ReadBlock(void *pkt, uint16_t size); + // write buffer static uint8_t *_writebuf; static const uint16_t _writebuf_size; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 291543eb01..b9c070866d 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -282,11 +282,11 @@ void DataFlash_Class::_print_log_entry(uint8_t msg_type, port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type); return; } - uint8_t msg_len = PGM_UINT8(&structure[i].msg_len); + uint8_t msg_len = PGM_UINT8(&structure[i].msg_len) - 3; uint8_t pkt[msg_len]; - ReadPacket(pkt, msg_len); + ReadBlock(pkt, msg_len); port->printf_P(PSTR("%S, "), structure[i].name); - for (uint8_t ofs=3, fmt_ofs=0; ofs