void DataFlash_File::_io_timer(void) { uint16_t _tail; if (_write_fd == -1 || !_initialised) { return; } uint16_t nbytes = BUF_AVAILABLE(_writebuf); if (nbytes == 0) { return; } uint32_t tnow = hal.scheduler->micros(); if (nbytes < 512 && tnow - _last_write_time < 2000000UL) { // write in 512 byte chunks, but always write at least once // per 2 seconds if data is available return; } _last_write_time = tnow; if (nbytes > 512) { // be kind to the FAT PX4 filesystem nbytes = 512; } if (_writebuf_head > _tail) { // only write to the end of the buffer nbytes = min(nbytes, _writebuf_size - _writebuf_head); } assert(_writebuf_head+nbytes <= _writebuf_size); ssize_t nwritten = ::write(_write_fd, &_writebuf[_writebuf_head], nbytes); if (nwritten <= 0) { //hal.console->printf("DataFlash write: %d %d\n", (int)nwritten, (int)errno); close(_write_fd); _write_fd = -1; _initialised = false; } else { BUF_ADVANCEHEAD(_writebuf, nwritten); if (hal.scheduler->millis() - last_fsync_ms > 10000) { last_fsync_ms = hal.scheduler->millis(); ::fsync(_write_fd); } } }
void DataFlash_File::_io_timer(void) { uint16_t _tail; if (_write_fd == -1 || !_initialised || _open_error) { return; } uint16_t nbytes = BUF_AVAILABLE(_writebuf); if (nbytes == 0) { return; } uint32_t tnow = hal.scheduler->micros(); if (nbytes < _writebuf_chunk && tnow - _last_write_time < 2000000UL) { // write in 512 byte chunks, but always write at least once // per 2 seconds if data is available return; } perf_begin(_perf_write); _last_write_time = tnow; if (nbytes > _writebuf_chunk) { // 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 if ((nbytes + _write_offset) % 512 != 0) { uint32_t ofs = (nbytes + _write_offset) % 512; if (ofs < nbytes) { nbytes -= ofs; } } assert(_writebuf_head+nbytes <= _writebuf_size); ssize_t nwritten = ::write(_write_fd, &_writebuf[_writebuf_head], nbytes); if (nwritten <= 0) { perf_count(_perf_errors); close(_write_fd); _write_fd = -1; _initialised = false; } else { _write_offset += nwritten; /* the best strategy for minimising 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. */ #if CONFIG_HAL_BOARD != HAL_BOARD_AVR_SITL ::fsync(_write_fd); #endif BUF_ADVANCEHEAD(_writebuf, nwritten); } perf_end(_perf_write); }