diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index dceb01b300..462a8765a2 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -29,14 +29,12 @@ #include #include #include -#include #if defined(__APPLE__) && defined(__MACH__) #include #include #elif !DATAFLASH_FILE_MINIMAL #include #endif - extern const AP_HAL::HAL& hal; #define MAX_LOG_FILES 500U @@ -58,7 +56,7 @@ DataFlash_File::DataFlash_File(DataFlash_Class &front, _open_error(false), _log_directory(log_directory), _cached_oldest_log(0), - _writebuf(NULL), + _writebuf(0), #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) // V1 gets IO errors with larger than 512 byte writes _writebuf_chunk(512), @@ -77,8 +75,6 @@ DataFlash_File::DataFlash_File(DataFlash_Class &front, #else _writebuf_chunk(4096), #endif - _writebuf_head(0), - _writebuf_tail(0), _last_write_time(0), _perf_write(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "DF_write")), _perf_fsync(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "DF_fsync")), @@ -87,7 +83,6 @@ DataFlash_File::DataFlash_File(DataFlash_Class &front, {} -// initialisation void DataFlash_File::Init() { DataFlash_Backend::Init(); @@ -100,7 +95,7 @@ void DataFlash_File::Init() AP_HAL::panic("Failed to create DataFlash_File semaphore"); return; } - + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // try to cope with an existing lowercase log directory // name. NuttX does not handle case insensitive VFAT well @@ -131,37 +126,27 @@ void DataFlash_File::Init() return; } #endif - - if (_writebuf != NULL) { - free(_writebuf); - _writebuf = NULL; - } // determine and limit file backend buffersize - uint8_t bufsize = _front._params.file_bufsize; + uint32_t bufsize = _front._params.file_bufsize; if (bufsize > 64) { - // we currently use uint16_t for the ringbuffer. Also, - // PixHawk has DMA limitaitons. - bufsize = 64; + bufsize = 64; // PixHawk has DMA limitations. } - _writebuf_size = bufsize * 1024; + bufsize *= 1024; - /* - if we can't allocate the full writebuf then try reducing it - until we can allocate it - */ - while (_writebuf == NULL && _writebuf_size >= _writebuf_chunk) { - hal.console->printf("DataFlash_File: buffer size=%u\n", (unsigned)_writebuf_size); - _writebuf = (uint8_t *)malloc(_writebuf_size); - if (_writebuf == NULL) { - _writebuf_size /= 2; - } + // If we can't allocate the full size, try to reduce it until we can allocate it + while (!_writebuf.set_size(bufsize) && bufsize >= _writebuf_chunk) { + hal.console->printf("DataFlash_File: Couldn't set buffer size to=%u\n", (unsigned)bufsize); + bufsize >>= 1; } - if (_writebuf == NULL) { + + if (!_writebuf.get_size()) { hal.console->printf("Out of memory for logging\n"); - return; + return; } - _writebuf_head = _writebuf_tail = 0; + + hal.console->printf("DataFlash_File: buffer size=%u\n", (unsigned)bufsize); + _initialised = true; hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&DataFlash_File::_io_timer, void)); } @@ -202,16 +187,15 @@ void DataFlash_File::periodic_fullrate(const uint32_t now) DataFlash_Backend::push_log_blocks(); } -uint16_t DataFlash_File::bufferspace_available() +uint32_t DataFlash_File::bufferspace_available() { - uint16_t _head; - const uint16_t space = BUF_SPACE(_writebuf); - const uint16_t crit = critical_message_reserved_space(); + const uint32_t space = _writebuf.space(); + const uint32_t crit = critical_message_reserved_space(); return (space > crit) ? space - crit : 0; } -// return true for CardInserted() if we successfully initialised +// return true for CardInserted() if we successfully initialized bool DataFlash_File::CardInserted(void) { return _initialised && !_open_error; @@ -489,8 +473,7 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b return false; } - uint16_t _head; - uint16_t space = BUF_SPACE(_writebuf); + uint16_t space = _writebuf.space(); if (_writing_startup_messages && _startup_messagewriter->fmt_done()) { @@ -520,26 +503,7 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b return false; } - if (_writebuf_tail < _head) { - // perform as single memcpy - assert(((uint32_t)_writebuf_tail)+size <= _writebuf_size); - memcpy(&_writebuf[_writebuf_tail], pBuffer, size); - BUF_ADVANCETAIL(_writebuf, size); - } else { - // perform as two memcpy calls - uint32_t n = _writebuf_size - _writebuf_tail; - if (n > size) n = size; - assert(((uint32_t)_writebuf_tail)+n <= _writebuf_size); - memcpy(&_writebuf[_writebuf_tail], pBuffer, n); - BUF_ADVANCETAIL(_writebuf, n); - pBuffer = (const void *)(((const uint8_t *)pBuffer) + n); - n = size - n; - if (n > 0) { - assert(((uint32_t)_writebuf_tail)+n <= _writebuf_size); - memcpy(&_writebuf[_writebuf_tail], pBuffer, n); - BUF_ADVANCETAIL(_writebuf, n); - } - } + _writebuf.write((uint8_t*)pBuffer, size); semaphore->give(); return true; } @@ -856,8 +820,7 @@ uint16_t DataFlash_File::start_new_log(void) } free(fname); _write_offset = 0; - _writebuf_head = 0; - _writebuf_tail = 0; + _writebuf.clear(); log_write_started = true; // now update lastlog.txt with the new log number @@ -1035,11 +998,9 @@ void DataFlash_File::ListAvailableLogs(AP_HAL::BetterStream *port) #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX void DataFlash_File::flush(void) { - uint16_t _tail; uint32_t tnow = AP_HAL::micros(); hal.scheduler->suspend_timer_procs(); - while (_write_fd != -1 && _initialised && !_open_error && - BUF_AVAILABLE(_writebuf)) { + while (_write_fd != -1 && _initialised && !_open_error && _writebuf.available()) { // convince the IO timer that it really is OK to write out // less than _writebuf_chunk bytes: if (tnow > 2000001) { // avoid resetting _last_write_time to 0 @@ -1056,12 +1017,11 @@ void DataFlash_File::flush(void) void DataFlash_File::_io_timer(void) { - uint16_t _tail; if (_write_fd == -1 || !_initialised || _open_error) { return; } - uint16_t nbytes = BUF_AVAILABLE(_writebuf); + uint32_t nbytes = _writebuf.available(); if (nbytes == 0) { return; } @@ -1089,13 +1049,12 @@ void DataFlash_File::_io_timer(void) // be kind to the FAT PX4 filesystem nbytes = _writebuf_chunk; } - if (_writebuf_head > _tail) { - // only write to the end of the buffer - nbytes = MIN(nbytes, _writebuf_size - _writebuf_head); - } - // try to align writes on a 512 byte boundary to avoid filesystem - // reads + uint32_t size; + const uint8_t *head = _writebuf.readptr(size); + nbytes = MIN(nbytes, size); + + // try to align writes on a 512 byte boundary to avoid filesystem reads if ((nbytes + _write_offset) % 512 != 0) { uint32_t ofs = (nbytes + _write_offset) % 512; if (ofs < nbytes) { @@ -1103,8 +1062,7 @@ void DataFlash_File::_io_timer(void) } } - assert(((uint32_t)_writebuf_head)+nbytes <= _writebuf_size); - ssize_t nwritten = ::write(_write_fd, &_writebuf[_writebuf_head], nbytes); + ssize_t nwritten = ::write(_write_fd, head, nbytes); if (nwritten <= 0) { hal.util->perf_count(_perf_errors); close(_write_fd); @@ -1112,13 +1070,13 @@ void DataFlash_File::_io_timer(void) _initialised = false; } else { _write_offset += nwritten; + _writebuf.advance(nwritten); /* - the best strategy for minimising corruption on microSD cards + the best strategy for minimizing corruption on microSD cards seems to be to write in 4k chunks and fsync the file on each chunk, ensuring the directory entry is updated after each write. */ - BUF_ADVANCEHEAD(_writebuf, nwritten); #if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE && CONFIG_HAL_BOARD != HAL_BOARD_QURT ::fsync(_write_fd); #endif @@ -1151,4 +1109,3 @@ bool DataFlash_File::logging_failed() const #endif // HAL_OS_POSIX_IO - diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index ba22109ae4..b44848bc69 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -8,6 +8,7 @@ #if HAL_OS_POSIX_IO +#include #include "DataFlash_Backend.h" #if CONFIG_HAL_BOARD == HAL_BOARD_QURT @@ -105,11 +106,8 @@ private: const float min_avail_space_percent = 10.0f; #endif // write buffer - uint8_t *_writebuf; - uint32_t _writebuf_size; // in bytes; may be == 65536, thus 32-bits + ByteBuffer _writebuf; const uint16_t _writebuf_chunk; - volatile uint16_t _writebuf_head; - volatile uint16_t _writebuf_tail; uint32_t _last_write_time; /* construct a file name given a log number. Caller must free. */ @@ -122,19 +120,19 @@ private: void _io_timer(void); - uint16_t critical_message_reserved_space() const { + uint32_t critical_message_reserved_space() const { // possibly make this a proportional to buffer size? - uint16_t ret = 1024; - if (ret > _writebuf_size) { + uint32_t ret = 1024; + if (ret > _writebuf.get_size()) { // in this case you will only get critical messages - ret = _writebuf_size; + ret = _writebuf.get_size(); } return ret; }; uint16_t non_messagewriter_message_reserved_space() const { // possibly make this a proportional to buffer size? uint16_t ret = 1024; - if (ret >= _writebuf_size) { + if (ret >= _writebuf.get_size()) { // need to allow messages out from the messagewriters. In // this case while you have a messagewriter you won't get // any other messages. This should be a corner case!