You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
989 lines
26 KiB
989 lines
26 KiB
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
/* |
|
DataFlash logging - file oriented variant |
|
|
|
This uses posix file IO to create log files called logs/NN.bin in the |
|
given directory |
|
|
|
SD Card Rates on PixHawk: |
|
- deletion rate seems to be ~50 files/second. |
|
- stat seems to be ~150/second |
|
- readdir loop of 511 entry directory ~62,000 microseconds |
|
*/ |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
#if HAL_OS_POSIX_IO |
|
#include "DataFlash_File.h" |
|
|
|
#include <sys/stat.h> |
|
#include <sys/types.h> |
|
#include <unistd.h> |
|
#include <errno.h> |
|
#include <stdlib.h> |
|
#include <sys/types.h> |
|
#include <sys/stat.h> |
|
#include <fcntl.h> |
|
#include <assert.h> |
|
#include <AP_Math/AP_Math.h> |
|
#include <stdio.h> |
|
#include <time.h> |
|
#include <dirent.h> |
|
#include <AP_HAL/utility/RingBuffer.h> |
|
#ifdef __APPLE__ |
|
#include <sys/param.h> |
|
#include <sys/mount.h> |
|
#else |
|
#include <sys/statfs.h> |
|
#endif |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
#define MAX_LOG_FILES 500U |
|
#define DATAFLASH_PAGE_SIZE 1024UL |
|
|
|
/* |
|
constructor |
|
*/ |
|
DataFlash_File::DataFlash_File(DataFlash_Class &front, const char *log_directory) : |
|
DataFlash_Backend(front), |
|
_write_fd(-1), |
|
_read_fd(-1), |
|
_read_fd_log_num(0), |
|
_read_offset(0), |
|
_write_offset(0), |
|
_initialised(false), |
|
_open_error(false), |
|
_log_directory(log_directory), |
|
_writebuf(NULL), |
|
_writebuf_size(16*1024), |
|
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) |
|
// V1 gets IO errors with larger than 512 byte writes |
|
_writebuf_chunk(512), |
|
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) |
|
_writebuf_chunk(512), |
|
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) |
|
_writebuf_chunk(512), |
|
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) |
|
_writebuf_chunk(512), |
|
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) |
|
_writebuf_chunk(512), |
|
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) |
|
_writebuf_chunk(512), |
|
#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) |
|
_writebuf_chunk(512), |
|
#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")), |
|
_perf_errors(hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "DF_errors")), |
|
_perf_overruns(hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "DF_overruns")) |
|
{} |
|
|
|
|
|
// initialisation |
|
void DataFlash_File::Init(const struct LogStructure *structure, uint8_t num_types) |
|
{ |
|
DataFlash_Backend::Init(structure, num_types); |
|
// create the log directory if need be |
|
int ret; |
|
struct stat st; |
|
|
|
#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 |
|
DIR *d = opendir("/fs/microsd/APM"); |
|
if (d != NULL) { |
|
for (struct dirent *de=readdir(d); de; de=readdir(d)) { |
|
if (strcmp(de->d_name, "logs") == 0) { |
|
rename("/fs/microsd/APM/logs", "/fs/microsd/APM/OLDLOGS"); |
|
break; |
|
} |
|
} |
|
closedir(d); |
|
} |
|
#endif |
|
|
|
const char* custom_dir = hal.util->get_custom_log_directory(); |
|
if (custom_dir != NULL){ |
|
_log_directory = custom_dir; |
|
} |
|
|
|
ret = stat(_log_directory, &st); |
|
if (ret == -1) { |
|
ret = mkdir(_log_directory, 0777); |
|
} |
|
if (ret == -1) { |
|
hal.console->printf("Failed to create log directory %s\n", _log_directory); |
|
return; |
|
} |
|
if (_writebuf != NULL) { |
|
free(_writebuf); |
|
_writebuf = NULL; |
|
} |
|
|
|
/* |
|
if we can't allocate the full writebuf then try reducing it |
|
until we can allocate it |
|
*/ |
|
while (_writebuf == NULL && _writebuf_size >= _writebuf_chunk) { |
|
_writebuf = (uint8_t *)malloc(_writebuf_size); |
|
if (_writebuf == NULL) { |
|
_writebuf_size /= 2; |
|
} |
|
} |
|
if (_writebuf == NULL) { |
|
hal.console->printf("Out of memory for logging\n"); |
|
return; |
|
} |
|
_writebuf_head = _writebuf_tail = 0; |
|
_initialised = true; |
|
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&DataFlash_File::_io_timer, void)); |
|
} |
|
|
|
bool DataFlash_File::file_exists(const char *filename) const |
|
{ |
|
struct stat st; |
|
if (stat(filename, &st) == -1) { |
|
// hopefully errno==ENOENT. If some error occurs it is |
|
// probably better to assume this file exists. |
|
return false; |
|
} |
|
return true; |
|
} |
|
|
|
bool DataFlash_File::log_exists(const uint16_t lognum) const |
|
{ |
|
char *filename = _log_file_name(lognum); |
|
if (filename == NULL) { |
|
// internal_error(); |
|
return false; // ?! |
|
} |
|
bool ret = file_exists(filename); |
|
free(filename); |
|
return ret; |
|
} |
|
|
|
void DataFlash_File::periodic_fullrate(const uint32_t now) |
|
{ |
|
DataFlash_Backend::push_log_blocks(); |
|
} |
|
|
|
uint16_t DataFlash_File::bufferspace_available() |
|
{ |
|
uint16_t _head; |
|
return (BUF_SPACE(_writebuf)) - critical_message_reserved_space(); |
|
} |
|
|
|
// return true for CardInserted() if we successfully initialised |
|
bool DataFlash_File::CardInserted(void) |
|
{ |
|
return _initialised && !_open_error; |
|
} |
|
|
|
// returns the amount of disk space available in _log_directory (in bytes) |
|
// returns -1 on error |
|
int64_t DataFlash_File::disk_space_avail() |
|
{ |
|
struct statfs stats; |
|
if (statfs(_log_directory, &stats) < 0) { |
|
return -1; |
|
} |
|
return (((int64_t)stats.f_bavail) * stats.f_bsize); |
|
} |
|
|
|
// returns the total amount of disk space (in use + available) in |
|
// _log_directory (in bytes). |
|
// returns -1 on error |
|
int64_t DataFlash_File::disk_space() |
|
{ |
|
struct statfs stats; |
|
if (statfs(_log_directory, &stats) < 0) { |
|
return -1; |
|
} |
|
return (((int64_t)stats.f_blocks) * stats.f_bsize); |
|
} |
|
|
|
// returns the available space in _log_directory as a percentage |
|
// returns -1.0f on error |
|
float DataFlash_File::avail_space_percent() |
|
{ |
|
int64_t avail = disk_space_avail(); |
|
if (avail == -1) { |
|
return -1.0f; |
|
} |
|
int64_t space = disk_space(); |
|
if (space == -1) { |
|
return -1.0f; |
|
} |
|
|
|
return (avail/(float)space) * 100; |
|
} |
|
|
|
// find_oldest_log - find oldest log in _log_directory |
|
// returns 0 if no log was found |
|
uint16_t DataFlash_File::find_oldest_log() |
|
{ |
|
uint16_t last_log_num = find_last_log(); |
|
if (last_log_num == 0) { |
|
return 0; |
|
} |
|
|
|
uint16_t current_oldest_log = 0; // 0 is invalid |
|
|
|
// We could count up to find_last_log(), but if people start |
|
// relying on the min_avail_space_percent feature we could end up |
|
// doing a *lot* of asprintf()s and stat()s |
|
DIR *d = opendir(_log_directory); |
|
if (d == NULL) { |
|
// internal_error(); |
|
return 0; |
|
} |
|
|
|
// we only remove files which look like xxx.BIN |
|
for (struct dirent *de=readdir(d); de; de=readdir(d)) { |
|
uint8_t length = strlen(de->d_name); |
|
if (length < 5) { |
|
// not long enough for \d+[.]BIN |
|
continue; |
|
} |
|
if (strncmp(&de->d_name[length-4], ".BIN", 4)) { |
|
// doesn't end in .BIN |
|
continue; |
|
} |
|
|
|
uint16_t thisnum = strtoul(de->d_name, NULL, 10); |
|
if (thisnum > MAX_LOG_FILES) { |
|
// ignore files above our official maximum... |
|
continue; |
|
} |
|
if (current_oldest_log == 0) { |
|
current_oldest_log = thisnum; |
|
} else { |
|
if (current_oldest_log <= last_log_num) { |
|
if (thisnum > last_log_num) { |
|
current_oldest_log = thisnum; |
|
} else if (thisnum < current_oldest_log) { |
|
current_oldest_log = thisnum; |
|
} |
|
} else { // current_oldest_log > last_log_num |
|
if (thisnum > last_log_num) { |
|
if (thisnum < current_oldest_log) { |
|
current_oldest_log = thisnum; |
|
} |
|
} |
|
} |
|
} |
|
} |
|
closedir(d); |
|
|
|
return current_oldest_log; |
|
} |
|
|
|
void DataFlash_File::Prep_MinSpace() |
|
{ |
|
const uint16_t first_log_to_remove = find_oldest_log(); |
|
if (first_log_to_remove == 0) { |
|
// no files to remove |
|
return; |
|
} |
|
|
|
uint16_t log_to_remove = first_log_to_remove; |
|
|
|
uint16_t count = 0; |
|
do { |
|
float avail = avail_space_percent(); |
|
if (is_equal(avail, -1.0f)) { |
|
// internal_error() |
|
break; |
|
} |
|
if (avail >= min_avail_space_percent) { |
|
break; |
|
} |
|
if (count++ > MAX_LOG_FILES+10) { |
|
// *way* too many deletions going on here. Possible internal error. |
|
// internal_error(); |
|
break; |
|
} |
|
char *filename_to_remove = _log_file_name(log_to_remove); |
|
if (filename_to_remove == NULL) { |
|
// internal_error(); |
|
break; |
|
} |
|
if (file_exists(filename_to_remove)) { |
|
hal.console->printf("Removing (%s) for minimum-space requirements (%.2f%% < %.0f%%)\n", |
|
filename_to_remove, avail, min_avail_space_percent); |
|
if (unlink(filename_to_remove) == -1) { |
|
hal.console->printf("Failed to remove %s: %s\n", filename_to_remove, strerror(errno)); |
|
free(filename_to_remove); |
|
if (errno == ENOENT) { |
|
// corruption - should always have a continuous |
|
// sequence of files... however, there may be still |
|
// files out there, so keep going. |
|
} else { |
|
// internal_error(); |
|
break; |
|
} |
|
} else { |
|
free(filename_to_remove); |
|
} |
|
} |
|
log_to_remove++; |
|
if (log_to_remove > MAX_LOG_FILES) { |
|
log_to_remove = 1; |
|
} |
|
} while (log_to_remove != first_log_to_remove); |
|
} |
|
|
|
void DataFlash_File::Prep() { |
|
if (hal.util->get_soft_armed()) { |
|
// do not want to do any filesystem operations while we are e.g. flying |
|
return; |
|
} |
|
Prep_MinSpace(); |
|
} |
|
|
|
bool DataFlash_File::NeedPrep() |
|
{ |
|
if (!CardInserted()) { |
|
// should not have been called?! |
|
return false; |
|
} |
|
|
|
if (avail_space_percent() < min_avail_space_percent) { |
|
return true; |
|
} |
|
|
|
return false; |
|
} |
|
|
|
/* |
|
construct a log file name given a log number. |
|
Note: Caller must free. |
|
*/ |
|
char *DataFlash_File::_log_file_name(const uint16_t log_num) const |
|
{ |
|
char *buf = NULL; |
|
if (asprintf(&buf, "%s/%u.BIN", _log_directory, (unsigned)log_num) == 0) { |
|
return NULL; |
|
} |
|
return buf; |
|
} |
|
|
|
/* |
|
return path name of the lastlog.txt marker file |
|
Note: Caller must free. |
|
*/ |
|
char *DataFlash_File::_lastlog_file_name(void) const |
|
{ |
|
char *buf = NULL; |
|
if (asprintf(&buf, "%s/LASTLOG.TXT", _log_directory) == 0) { |
|
return NULL; |
|
} |
|
return buf; |
|
} |
|
|
|
|
|
// remove all log files |
|
void DataFlash_File::EraseAll() |
|
{ |
|
uint16_t log_num; |
|
stop_logging(); |
|
for (log_num=1; log_num<=MAX_LOG_FILES; log_num++) { |
|
char *fname = _log_file_name(log_num); |
|
if (fname == NULL) { |
|
break; |
|
} |
|
unlink(fname); |
|
free(fname); |
|
} |
|
char *fname = _lastlog_file_name(); |
|
if (fname != NULL) { |
|
unlink(fname); |
|
free(fname); |
|
} |
|
} |
|
|
|
/* Write a block of data at current offset */ |
|
bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) |
|
{ |
|
if (_write_fd == -1 || !_initialised || _open_error || !_writes_enabled) { |
|
return false; |
|
} |
|
|
|
if (! WriteBlockCheckStartupMessages()) { |
|
_dropped++; |
|
return false; |
|
} |
|
|
|
uint16_t _head; |
|
uint16_t space = BUF_SPACE(_writebuf); |
|
|
|
if (_writing_startup_messages && |
|
_front._startup_messagewriter.fmt_done()) { |
|
// the state machine has called us, and it has finished |
|
// writing format messages out. It can always get back to us |
|
// with more messages later, so let's leave room for other |
|
// things: |
|
if (space < non_messagewriter_message_reserved_space()) { |
|
// this message isn't dropped, it will be sent again... |
|
return false; |
|
} |
|
} else { |
|
// we reserve some amount of space for critical messages: |
|
if (!is_critical && space < critical_message_reserved_space()) { |
|
_dropped++; |
|
return false; |
|
} |
|
} |
|
|
|
// if no room for entire message - drop it: |
|
if (space < size) { |
|
hal.util->perf_count(_perf_overruns); |
|
_dropped++; |
|
return false; |
|
} |
|
|
|
if (_writebuf_tail < _head) { |
|
// perform as single memcpy |
|
assert(_writebuf_tail+size <= _writebuf_size); |
|
memcpy(&_writebuf[_writebuf_tail], pBuffer, size); |
|
BUF_ADVANCETAIL(_writebuf, size); |
|
} else { |
|
// perform as two memcpy calls |
|
uint16_t n = _writebuf_size - _writebuf_tail; |
|
if (n > size) n = size; |
|
assert(_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(_writebuf_tail+n <= _writebuf_size); |
|
memcpy(&_writebuf[_writebuf_tail], pBuffer, n); |
|
BUF_ADVANCETAIL(_writebuf, n); |
|
} |
|
} |
|
return true; |
|
} |
|
|
|
/* |
|
read a packet. The header bytes have already been read. |
|
*/ |
|
bool DataFlash_File::ReadBlock(void *pkt, uint16_t size) |
|
{ |
|
if (_read_fd == -1 || !_initialised || _open_error) { |
|
return false; |
|
} |
|
|
|
memset(pkt, 0, size); |
|
if (::read(_read_fd, pkt, size) != size) { |
|
return false; |
|
} |
|
_read_offset += size; |
|
return true; |
|
} |
|
|
|
|
|
/* |
|
find the highest log number |
|
*/ |
|
uint16_t DataFlash_File::find_last_log() |
|
{ |
|
unsigned ret = 0; |
|
char *fname = _lastlog_file_name(); |
|
if (fname == NULL) { |
|
return ret; |
|
} |
|
FILE *f = ::fopen(fname, "r"); |
|
free(fname); |
|
if (f != NULL) { |
|
char buf[10]; |
|
memset(buf, 0, sizeof(buf)); |
|
// PX4 doesn't have fscanf() |
|
if (fread(buf, 1, sizeof(buf)-1, f) > 0) { |
|
sscanf(buf, "%u", &ret); |
|
} |
|
fclose(f); |
|
} |
|
return ret; |
|
} |
|
|
|
uint32_t DataFlash_File::_get_log_size(const uint16_t log_num) const |
|
{ |
|
char *fname = _log_file_name(log_num); |
|
if (fname == NULL) { |
|
return 0; |
|
} |
|
struct stat st; |
|
if (::stat(fname, &st) != 0) { |
|
free(fname); |
|
return 0; |
|
} |
|
free(fname); |
|
return st.st_size; |
|
} |
|
|
|
uint32_t DataFlash_File::_get_log_time(const uint16_t log_num) const |
|
{ |
|
char *fname = _log_file_name(log_num); |
|
if (fname == NULL) { |
|
return 0; |
|
} |
|
struct stat st; |
|
if (::stat(fname, &st) != 0) { |
|
free(fname); |
|
return 0; |
|
} |
|
free(fname); |
|
return st.st_mtime; |
|
} |
|
|
|
/* |
|
convert a list entry number back into a log number (which can then |
|
be converted into a filename). A "list entry number" is a sequence |
|
where the oldest log has a number of 1, the second-from-oldest 2, |
|
and so on. Thus the highest list entry number is equal to the |
|
number of logs. |
|
*/ |
|
uint16_t DataFlash_File::_log_num_from_list_entry(const uint16_t list_entry) |
|
{ |
|
uint16_t oldest_log = find_oldest_log(); |
|
if (oldest_log == 0) { |
|
// We don't have any logs... |
|
return 0; |
|
} |
|
|
|
uint32_t log_num = oldest_log + list_entry - 1; |
|
if (log_num > MAX_LOG_FILES) { |
|
log_num -= MAX_LOG_FILES; |
|
} |
|
return (uint16_t)log_num; |
|
} |
|
|
|
/* |
|
find the number of pages in a log |
|
*/ |
|
void DataFlash_File::get_log_boundaries(const uint16_t list_entry, uint16_t & start_page, uint16_t & end_page) |
|
{ |
|
const uint16_t log_num = _log_num_from_list_entry(list_entry); |
|
if (log_num == 0) { |
|
// that failed - probably no logs |
|
start_page = 0; |
|
end_page = 0; |
|
return; |
|
} |
|
|
|
start_page = 0; |
|
end_page = _get_log_size(log_num) / DATAFLASH_PAGE_SIZE; |
|
} |
|
|
|
/* |
|
find the number of pages in a log |
|
*/ |
|
int16_t DataFlash_File::get_log_data(const uint16_t list_entry, const uint16_t page, const uint32_t offset, const uint16_t len, uint8_t *data) |
|
{ |
|
if (!_initialised || _open_error) { |
|
return -1; |
|
} |
|
|
|
const uint16_t log_num = _log_num_from_list_entry(list_entry); |
|
if (log_num == 0) { |
|
// that failed - probably no logs |
|
return -1; |
|
} |
|
|
|
if (_read_fd != -1 && log_num != _read_fd_log_num) { |
|
::close(_read_fd); |
|
_read_fd = -1; |
|
} |
|
if (_read_fd == -1) { |
|
char *fname = _log_file_name(log_num); |
|
if (fname == NULL) { |
|
return -1; |
|
} |
|
stop_logging(); |
|
_read_fd = ::open(fname, O_RDONLY); |
|
if (_read_fd == -1) { |
|
_open_error = true; |
|
int saved_errno = errno; |
|
::printf("Log read open fail for %s - %s\n", |
|
fname, strerror(saved_errno)); |
|
hal.console->printf("Log read open fail for %s - %s\n", |
|
fname, strerror(saved_errno)); |
|
free(fname); |
|
return -1; |
|
} |
|
free(fname); |
|
_read_offset = 0; |
|
_read_fd_log_num = log_num; |
|
} |
|
uint32_t ofs = page * (uint32_t)DATAFLASH_PAGE_SIZE + offset; |
|
|
|
/* |
|
this rather strange bit of code is here to work around a bug |
|
in file offsets in NuttX. Every few hundred blocks of reads |
|
(starting at around 350k into a file) NuttX will get the |
|
wrong offset for sequential reads. The offset it gets is |
|
typically 128k earlier than it should be. It turns out that |
|
calling lseek() with 0 offset and SEEK_CUR works around the |
|
bug. We can remove this once we find the real bug. |
|
*/ |
|
if (ofs / 4096 != (ofs+len) / 4096) { |
|
off_t seek_current = ::lseek(_read_fd, 0, SEEK_CUR); |
|
if (seek_current != (off_t)_read_offset) { |
|
::lseek(_read_fd, _read_offset, SEEK_SET); |
|
} |
|
} |
|
|
|
if (ofs != _read_offset) { |
|
::lseek(_read_fd, ofs, SEEK_SET); |
|
_read_offset = ofs; |
|
} |
|
int16_t ret = (int16_t)::read(_read_fd, data, len); |
|
if (ret > 0) { |
|
_read_offset += ret; |
|
} |
|
return ret; |
|
} |
|
|
|
/* |
|
find size and date of a log |
|
*/ |
|
void DataFlash_File::get_log_info(const uint16_t list_entry, uint32_t &size, uint32_t &time_utc) |
|
{ |
|
uint16_t log_num = _log_num_from_list_entry(list_entry); |
|
if (log_num == 0) { |
|
// that failed - probably no logs |
|
size = 0; |
|
time_utc = 0; |
|
return; |
|
} |
|
|
|
size = _get_log_size(log_num); |
|
time_utc = _get_log_time(log_num); |
|
} |
|
|
|
|
|
|
|
/* |
|
get the number of logs - note that the log numbers must be consecutive |
|
*/ |
|
uint16_t DataFlash_File::get_num_logs() |
|
{ |
|
uint16_t ret = 0; |
|
uint16_t high = find_last_log(); |
|
uint16_t i; |
|
for (i=high; i>0; i--) { |
|
if (! log_exists(i)) { |
|
break; |
|
} |
|
ret++; |
|
} |
|
if (i == 0) { |
|
for (i=MAX_LOG_FILES; i>high; i--) { |
|
if (! log_exists(i)) { |
|
break; |
|
} |
|
ret++; |
|
} |
|
} |
|
return ret; |
|
} |
|
|
|
/* |
|
stop logging |
|
*/ |
|
void DataFlash_File::stop_logging(void) |
|
{ |
|
if (_write_fd != -1) { |
|
int fd = _write_fd; |
|
_write_fd = -1; |
|
log_write_started = false; |
|
::close(fd); |
|
} |
|
} |
|
|
|
|
|
/* |
|
start writing to a new log file |
|
*/ |
|
uint16_t DataFlash_File::start_new_log(void) |
|
{ |
|
stop_logging(); |
|
|
|
if (_open_error) { |
|
// we have previously failed to open a file - don't try again |
|
// to prevent us trying to open files while in flight |
|
return 0xFFFF; |
|
} |
|
|
|
if (_read_fd != -1) { |
|
::close(_read_fd); |
|
_read_fd = -1; |
|
} |
|
|
|
uint16_t log_num = find_last_log(); |
|
// re-use empty logs if possible |
|
if (_get_log_size(log_num) > 0 || log_num == 0) { |
|
log_num++; |
|
} |
|
if (log_num > MAX_LOG_FILES) { |
|
log_num = 1; |
|
} |
|
char *fname = _log_file_name(log_num); |
|
_write_fd = ::open(fname, O_WRONLY|O_CREAT|O_TRUNC, 0666); |
|
if (_write_fd == -1) { |
|
_initialised = false; |
|
_open_error = true; |
|
int saved_errno = errno; |
|
::printf("Log open fail for %s - %s\n", |
|
fname, strerror(saved_errno)); |
|
hal.console->printf("Log open fail for %s - %s\n", |
|
fname, strerror(saved_errno)); |
|
free(fname); |
|
return 0xFFFF; |
|
} |
|
free(fname); |
|
_write_offset = 0; |
|
_writebuf_head = 0; |
|
_writebuf_tail = 0; |
|
log_write_started = true; |
|
|
|
// now update lastlog.txt with the new log number |
|
fname = _lastlog_file_name(); |
|
FILE *f = ::fopen(fname, "w"); |
|
fprintf(f, "%u\r\n", (unsigned)log_num); |
|
fclose(f); |
|
free(fname); |
|
|
|
return log_num; |
|
} |
|
|
|
/* |
|
Read the log and print it on port |
|
*/ |
|
void DataFlash_File::LogReadProcess(const uint16_t list_entry, |
|
uint16_t start_page, uint16_t end_page, |
|
print_mode_fn print_mode, |
|
AP_HAL::BetterStream *port) |
|
{ |
|
uint8_t log_step = 0; |
|
if (!_initialised || _open_error) { |
|
return; |
|
} |
|
|
|
const uint16_t log_num = _log_num_from_list_entry(list_entry); |
|
if (log_num == 0) { |
|
return; |
|
} |
|
|
|
if (_read_fd != -1) { |
|
::close(_read_fd); |
|
_read_fd = -1; |
|
} |
|
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_fd_log_num = log_num; |
|
_read_offset = 0; |
|
if (start_page != 0) { |
|
::lseek(_read_fd, start_page * DATAFLASH_PAGE_SIZE, SEEK_SET); |
|
_read_offset = start_page * DATAFLASH_PAGE_SIZE; |
|
} |
|
|
|
uint8_t log_counter = 0; |
|
|
|
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; |
|
_print_log_entry(data, print_mode, port); |
|
log_counter++; |
|
if (log_counter == 10) { |
|
log_counter = 0; |
|
::lseek(_read_fd, 0, SEEK_CUR); |
|
} |
|
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 |
|
and look at it on your PC |
|
*/ |
|
void DataFlash_File::DumpPageInfo(AP_HAL::BetterStream *port) |
|
{ |
|
port->printf_P(PSTR("DataFlash: num_logs=%u\n"), |
|
(unsigned)get_num_logs()); |
|
} |
|
|
|
void DataFlash_File::ShowDeviceInfo(AP_HAL::BetterStream *port) |
|
{ |
|
port->printf_P(PSTR("DataFlash logs stored in %s\n"), |
|
_log_directory); |
|
} |
|
|
|
|
|
/* |
|
list available log numbers |
|
*/ |
|
void DataFlash_File::ListAvailableLogs(AP_HAL::BetterStream *port) |
|
{ |
|
uint16_t num_logs = get_num_logs(); |
|
|
|
if (num_logs == 0) { |
|
port->printf_P(PSTR("\nNo logs\n\n")); |
|
return; |
|
} |
|
port->printf_P(PSTR("\n%u logs\n"), (unsigned)num_logs); |
|
|
|
for (uint16_t i=1; i<=num_logs; i++) { |
|
uint16_t log_num = _log_num_from_list_entry(i); |
|
char *filename = _log_file_name(log_num); |
|
if (filename != NULL) { |
|
struct stat st; |
|
if (stat(filename, &st) == 0) { |
|
struct tm *tm = gmtime(&st.st_mtime); |
|
port->printf_P(PSTR("Log %u in %s of size %u %u/%u/%u %u:%u\n"), |
|
(unsigned)i, |
|
filename, |
|
(unsigned)st.st_size, |
|
(unsigned)tm->tm_year+1900, |
|
(unsigned)tm->tm_mon+1, |
|
(unsigned)tm->tm_mday, |
|
(unsigned)tm->tm_hour, |
|
(unsigned)tm->tm_min); |
|
} |
|
free(filename); |
|
} |
|
} |
|
port->println(); |
|
} |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
|
void DataFlash_File::flush(void) |
|
{ |
|
uint16_t _tail; |
|
uint32_t tnow = hal.scheduler->micros(); |
|
hal.scheduler->suspend_timer_procs(); |
|
while (_write_fd != -1 && _initialised && !_open_error && |
|
BUF_AVAILABLE(_writebuf)) { |
|
// convince the IO timer that it really is OK to write out |
|
// less than _writebuf_chunk bytes: |
|
_last_write_time = tnow - 2000000; |
|
_io_timer(); |
|
} |
|
hal.scheduler->resume_timer_procs(); |
|
if (_write_fd != -1) { |
|
::fsync(_write_fd); |
|
} |
|
} |
|
#endif |
|
|
|
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 _writebuf_chunk-sized chunks, but always write at |
|
// least once per 2 seconds if data is available |
|
return; |
|
} |
|
|
|
hal.util->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) { |
|
hal.util->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. |
|
*/ |
|
BUF_ADVANCEHEAD(_writebuf, nwritten); |
|
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE |
|
::fsync(_write_fd); |
|
#endif |
|
} |
|
hal.util->perf_end(_perf_write); |
|
} |
|
|
|
#endif // HAL_OS_POSIX_IO |
|
|
|
|