diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 45652bd393..7c04a20b4c 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -3,8 +3,13 @@ #include "DataFlash_Backend.h" #include "DataFlash_File.h" +#include "DataFlash_File_sd.h" #include "DataFlash_MAVLink.h" #include +#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT +#include "DataFlash_Revo.h" +#endif + DataFlash_Class *DataFlash_Class::_instance; @@ -76,7 +81,7 @@ void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_ty _num_types = num_types; _structures = structures; -#if (HAL_OS_POSIX_IO || HAL_OS_FATFS_IO) && defined(HAL_BOARD_LOG_DIRECTORY) +#if defined(HAL_BOARD_LOG_DIRECTORY) if (_params.backend_types == DATAFLASH_BACKEND_FILE || _params.backend_types == DATAFLASH_BACKEND_BOTH) { DFMessageWriter_DFLogStart *message_writer = @@ -92,6 +97,24 @@ void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_ty _next_backend++; } } +#elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT // restore dataflash logs + + if (_params.backend_types == DATAFLASH_BACKEND_FILE || + _params.backend_types == DATAFLASH_BACKEND_BOTH) { + + DFMessageWriter_DFLogStart *message_writer = + new DFMessageWriter_DFLogStart(_firmware_string); + if (message_writer != nullptr) { + + backends[_next_backend] = new DataFlash_Revo(*this, message_writer); + } + + if (backends[_next_backend] == nullptr) { + hal.console->printf("Unable to open DataFlash_Revo"); + } else { + _next_backend++; + } + } #endif #if DATAFLASH_MAVLINK_SUPPORT diff --git a/libraries/DataFlash/DataFlash_File_sd.cpp b/libraries/DataFlash/DataFlash_File_sd.cpp new file mode 100644 index 0000000000..672ca9c75a --- /dev/null +++ b/libraries/DataFlash/DataFlash_File_sd.cpp @@ -0,0 +1,1191 @@ +/* + DataFlash logging - file oriented variant + + This uses SD library to create log files called logs/NN.bin in the + given directory + + SD Card Rates + - deletion rate seems to be ~50 files/second. + - stat seems to be ~150/second + - readdir loop of 511 entry directory ~62,000 microseconds + */ + + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT && (defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS)) + +#include "DataFlash_File_sd.h" + +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#if defined(BOARD_DATAFLASH_FATFS) + #define MAX_LOG_FILES 9U +#else + #define MAX_LOG_FILES 50U +#endif + +#define DATAFLASH_PAGE_SIZE 1024UL + +#define MAX_FILE_SIZE 2048 * 1024L // not more 2MB + +/* + constructor + */ +DataFlash_File::DataFlash_File(DataFlash_Class &front, + DFMessageWriter_DFLogStart *writer, + const char *log_directory) : + DataFlash_Backend(front, writer), + _write_fd(File()), + _read_fd(File()), + _read_fd_log_num(0), + _read_offset(0), + _write_offset(0), + _open_error(false), + _log_directory(log_directory), + _cached_oldest_log(0), + _last_oldest_log(0), + _writebuf(0), + _writebuf_chunk(4096), + _last_write_time(0), + has_data(false), + _busy(false) +{} + + +void DataFlash_File::Init() +{ + DataFlash_Backend::Init(); + + if(HAL_F4Light::state.sd_busy) return; // SD mounted via USB + + semaphore = hal.util->new_semaphore(); + if (semaphore == nullptr) { + AP_HAL::panic("Failed to create DataFlash_File semaphore"); + return; + } + + + // create the log directory if need be + const char* custom_dir = hal.util->get_custom_log_directory(); + if (custom_dir != nullptr){ + _log_directory = custom_dir; + } + + if (! SD.exists(_log_directory) ) { + char buf[80]; + const char *cp, *rp; + char *wp; + for(cp=_log_directory + strlen(_log_directory);cp>_log_directory && *cp != '/';cp--){ } + for(wp=buf, rp=_log_directory; rp 64) { + bufsize = 64; // DMA limitations. + } + bufsize *= 1024; + + // 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) { + printf("DataFlash_File: Couldn't set buffer size to=%u\n", (unsigned)bufsize); + bufsize /= 2; + } + + if (!_writebuf.get_size()) { + printf("Out of memory for logging\n"); + return; + } + + printf("DataFlash_File: buffer size=%u\n", (unsigned)bufsize); + + _initialised = true; + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&DataFlash_File::_io_timer, void)); +} + +bool DataFlash_File::file_exists(const char *filename) const +{ + return SD.exists(filename); +} + +bool DataFlash_File::log_exists(const uint16_t lognum) const +{ + char *filename = _log_file_name(lognum); + if (filename == nullptr) { + // internal_error(); + return false; // ?! + } + bool ret = file_exists(filename); + free(filename); + return ret; +} + +void DataFlash_File::periodic_1Hz(const uint32_t now) +{ + if (!(_write_fd) || !_initialised || _open_error || _busy) return; // too early + + if (!io_thread_alive()) { + gcs().send_text(MAV_SEVERITY_WARNING, "No IO Thread Heartbeat"); + // If you try to close the file here then it will almost + // certainly block. Since this is the main thread, this is + // likely to cause a crash. +// _write_fd.close(); + _write_fd.sync(); + printf("\nLoging stopped\n"); + _initialised = false; + } +} + +void DataFlash_File::periodic_fullrate(const uint32_t now) +{ + DataFlash_Backend::push_log_blocks(); +} + +uint32_t DataFlash_File::bufferspace_available() +{ + 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 initialized +bool DataFlash_File::CardInserted(void) const +{ + return _initialised && !_open_error && !HAL_F4Light::state.sd_busy; +} + +// returns the available space in _log_directory as a percentage +// returns -1.0f on error +float DataFlash_File::avail_space_percent(uint32_t *free) +{ + + uint32_t space; + int32_t avail = SD.getfree(_log_directory, &space); + if(free) *free = avail; + if (avail == -1) { + return -1.0f; + } + + return (avail/(float)space) * 100; +} + +#if 0 // why such hard? + +// find_oldest_log - find oldest log in _log_directory +// returns 0 if no log was found +uint16_t DataFlash_File::find_oldest_log() +{ + if (_cached_oldest_log != 0) { + return _cached_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 + File dir = SD.open(_log_directory); + if (!dir) { + // internal_error(); + printf("error opening logs dir: %s", SD.strError(SD.lastError)); + return 0; + } + + // we only count files which look like xxx.BIN + while(1){ + File de=dir.openNextFile(); + if(!de) { + if(SD.lastError){ + printf("error scanning logs: %s", SD.strError(SD.lastError)); + } + break; + } + + char *nm = de.name(); + de.close(); + + uint8_t length = strlen(nm); + if (length < 5) { + // not long enough for \d+[.]BIN + continue; + } + if (strncmp(&nm[length-4], ".BIN", 4)) { + // doesn't end in .BIN + continue; + } + + uint16_t thisnum = strtoul(nm, nullptr, 10); + 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; + } + } + } + } + } + dir.close(); + _cached_oldest_log = current_oldest_log; + + return current_oldest_log; +} + +#else + +// result cached later +uint16_t DataFlash_File::find_oldest_log() +{ + + if (_cached_oldest_log != 0) { + return _cached_oldest_log; + } + + uint16_t i; + if(_last_oldest_log >=MAX_LOG_FILES-1) _last_oldest_log=1; + for (i=_last_oldest_log; i_sd_format){ + printf("error getting free space, formatting!\n"); + gcs().send_text(MAV_SEVERITY_WARNING,"error getting free space, formatting!\n"); + SD.format(_log_directory); + return; + } +#endif + break; + } + if (avail >= min_avail_space_percent && free_sp*512 >= MAX_FILE_SIZE) { // not less 2MB - space for one file + break; + } + if (count++ > MAX_LOG_FILES+10) { + // *way* too many deletions going on here. Possible internal error. + break; + } + char *filename_to_remove = _log_file_name(log_to_remove); + if (filename_to_remove == nullptr) { + break; + } + if (SD.exists(filename_to_remove)) { + printf("Removing (%s) for minimum-space requirements (%.2f%% < %.0f%%) %.1fMb\n", + filename_to_remove, (double)avail, (double)min_avail_space_percent, free_sp/(1024.*2)); + if (!SD.remove(filename_to_remove)) { + printf("Failed to remove %s: %s\n", filename_to_remove, SD.strError(SD.lastError)); + } + 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); + + } +// check the result +#if defined(BOARD_DATAFLASH_FATFS) + float avail = avail_space_percent(); + if (avail <= 0) { // erase don't helps + printf("erase don't get free space, formatting!\n"); + SD.format(_log_directory); + } +#endif + +} + + +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 = (char *)malloc(256); + if(!buf) return nullptr; + + sprintf(buf, "%s/%u.BIN", _log_directory, (unsigned)log_num); + + 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 = (char *)malloc(256); + if(!buf) return nullptr; + + sprintf(buf, "%s/LASTLOG.TXT", _log_directory); + return buf; +} + + +// remove all log files +void DataFlash_File::EraseAll() +{ + uint16_t log_num; + const bool was_logging = (_write_fd != -1); + stop_logging(); + + for (log_num=1; log_num<=MAX_LOG_FILES; log_num++) { + char *fname = _log_file_name(log_num); + if (fname == nullptr) { + break; + } + SD.remove(fname); + free(fname); + } + char *fname = _lastlog_file_name(); + if (fname != nullptr) { + SD.remove(fname); + free(fname); + } + + _cached_oldest_log = 0; + + if (was_logging) { + start_new_log(); + } +} + + +bool DataFlash_File::WritesOK() const +{ + if (!_write_fd) { + return false; + } + if (_open_error) { + return false; + } + return true; +} + + +bool DataFlash_File::StartNewLogOK() const +{ + if (_open_error) { + return false; + } + return DataFlash_Backend::StartNewLogOK(); +} + + +/* Write a block of data at current offset */ +bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) +{ + if (! WriteBlockCheckStartupMessages()) { + _dropped++; + return false; + } + + if (!semaphore->take(1)) { + return false; + } + + uint32_t space = _writebuf.space(); + + if (_writing_startup_messages && + _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... + semaphore->give(); + return false; + } + } else { + // we reserve some amount of space for critical messages: + if (!is_critical && space < critical_message_reserved_space()) { +// printf("dropping NC block! size=%d\n", size); + _dropped++; + semaphore->give(); + return false; + } + } + + // if no room for entire message - drop it: + if (space < size) { +// hal.util->perf_count(_perf_overruns); + printf("dropping block! size=%d\n", size); + + _dropped++; + semaphore->give(); + return false; + } + + _writebuf.write((uint8_t*)pBuffer, size); + has_data=true; + + semaphore->give(); + return true; +} + +/* + read a packet. The header bytes have already been read. +*/ +bool DataFlash_File::ReadBlock(void *pkt, uint16_t size) +{ + if (!(_read_fd) || !_initialised || _open_error) { + return false; + } + + memset(pkt, 0, size); + if (_read_fd.read(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 == nullptr) { + return ret; + } + File fd = SD.open(fname, FILE_READ); + free(fname); + if (fd) { + char buf[10]; + memset(buf, 0, sizeof(buf)); + if (fd.read(buf, sizeof(buf)-1) > 0) { + ret = strtoul(buf, nullptr, 10); // зачем тащить толстую функцию зря +// sscanf(buf, "%u", &ret); + } + fd.close(); + } + return ret; +} + +uint32_t DataFlash_File::_get_log_size(const uint16_t log_num) const +{ + char *fname = _log_file_name(log_num); + if (fname == nullptr) { + return 0; + } + + File fd = SD.open(fname, FILE_READ); + free(fname); + + if(!fd) return 0; + + uint32_t sz= fd.size(); + fd.close(); + + return sz; +} + +uint32_t DataFlash_File::_get_log_time(const uint16_t log_num) const +{ + char *fname = _log_file_name(log_num); + if (fname == nullptr) { + return 0; + } + + FILINFO fno; + + int8_t ret = SD.stat(fname, &fno); + free(fname); + + if(ret<0) return 0; + + uint16_t date=fno.fdate, + time=fno.ftime; + + + struct tm t; + + t.tm_sec = FAT_SECOND(time); // seconds after the minute 0-61* + t.tm_min = FAT_MINUTE(time); // minutes after the hour 0-59 + t.tm_hour = FAT_HOUR(time); // hours since midnight 0-23 + t.tm_mday = FAT_DAY(date); // day of the month 1-31 + t.tm_mon = FAT_MONTH(date); // months since January 0-11 + t.tm_year = FAT_YEAR(date); // years since 1900 +// t.tm_yday int days since January 1 0-365 + t.tm_isdst =false; + + + return to_timestamp(&t); +} + +/* + 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; + } + while(!log_exists(log_num)){ // skip gaps + log_num++; + if(log_num>MAX_LOG_FILES) { + log_num=MAX_LOG_FILES; + break; + } + } + + 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) { + if (! log_exists(log_num)) { + // 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; +} + +/* + retrieve data from a log file + */ +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 && log_num != _read_fd_log_num) { + _read_fd.close(); + } + if (!(_read_fd)) { + char *fname = _log_file_name(log_num); + if (fname == nullptr) { + return -1; + } + stop_logging(); + _read_fd = SD.open(fname, O_RDONLY); + if (!(_read_fd)) { + _open_error = true; + + printf("Log read open fail for %s: %s\n", fname, SD.strError(SD.lastError)); + free(fname); + return -1; + } + free(fname); + _read_offset = 0; + _read_fd_log_num = log_num; + } + + int16_t ret = (int16_t)_read_fd.read(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) { + if (! log_exists(log_num)) { + + // 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 [no more - 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)) { + ret++; + if(_cached_oldest_log == 0 || i<_cached_oldest_log) + _cached_oldest_log=i; + } + } + + if (i == 0) { + for (i=MAX_LOG_FILES; i>high; i--) { + if ( log_exists(i)) { + ret++; + if(_cached_oldest_log == 0 || i<_cached_oldest_log) + _cached_oldest_log=i; + } + } + } + return ret; +} + +/* + stop logging + */ +void DataFlash_File::stop_logging(void) +{ + if (_write_fd) { + _write_fd.close(); + } +} + + +void DataFlash_File::PrepForArming() +{ + if (logging_started()) { + return; + } + start_new_log(); +} + +/* + start writing to a new log file + */ +uint16_t DataFlash_File::start_new_log(void) +{ + stop_logging(); + + start_new_log_reset_variables(); + + 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) { + _read_fd.close(); + } + + 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; + } + _cached_oldest_log = 0; + + bool was_ovf=false; + + char *fname; + while(1) { // try to create log file + + fname = _log_file_name(log_num); + if (fname == nullptr) { + _open_error = true; + return 0xFFFF; // no memory + } + + _write_fd = SD.open(fname, O_WRITE|O_CREAT|O_TRUNC); + + if (_write_fd) { // file opened + free(fname); + break; + } + + // opening failed + printf("Log open fail for %s: %s\n",fname, SD.strError(SD.lastError)); + free(fname); + + log_num++; // if not at end - try to open next log + + if (log_num >= MAX_LOG_FILES) { + log_num = 1; + if(was_ovf) { + _initialised = false; // no space + printf("\nLoging stopped\n"); + return 0xFFFF; + } + was_ovf=true; + } + } + _write_offset = 0; + _writebuf.clear(); + has_data = false; + + // now update lastlog.txt with the new log number + fname = _lastlog_file_name(); + + File fd = SD.open(fname, O_WRITE|O_CREAT); + free(fname); + if (!fd) { + return 0xFFFF; + } + + char buf[30]; + snprintf(buf, sizeof(buf), "%u\r\n", (unsigned)log_num); + const ssize_t to_write = strlen(buf); + const ssize_t written = fd.write((uint8_t *)buf, to_write); + fd.close(); + + if (written < to_write) { + return 0xFFFF; + } + + 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) { + _read_fd.close(); + } + char *fname = _log_file_name(log_num); + if (fname == nullptr) { + return; + } + _read_fd = SD.open(fname, O_RDONLY); + free(fname); + if (!(_read_fd)) { + return; + } + _read_fd_log_num = log_num; + _read_offset = 0; + if (start_page != 0) { + if (!_read_fd.seek(start_page * DATAFLASH_PAGE_SIZE)) { + _read_fd.close(); + return; + } + _read_offset = start_page * DATAFLASH_PAGE_SIZE; + } + + uint8_t log_counter = 0; + + while (true) { + uint8_t data; + if (_read_fd.read(&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++; + break; + } + if (_read_offset >= (end_page+1) * DATAFLASH_PAGE_SIZE) { + break; + } + } + + _read_fd.close(); +} + +/* + 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 or mount FC as massStorage + and look at it on your PC + */ +void DataFlash_File::DumpPageInfo(AP_HAL::BetterStream *port) +{ + port->printf("DataFlash: num_logs=%u\n", (unsigned)get_num_logs()); +} + +void DataFlash_File::ShowDeviceInfo(AP_HAL::BetterStream *port) +{ + port->printf("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("\nNo logs\n\n"); + return; + } + port->printf("\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 != nullptr) { + if(file_exists(filename)) { + uint32_t time=_get_log_time(log_num); + struct tm *tm = gmtime((const time_t*)&time); + port->printf("Log %u in %s of size %u %u/%u/%u %u:%u\n", + (unsigned)i, + filename, + (unsigned)_get_log_size(log_num), + (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(); +} + + +void DataFlash_File::_io_timer(void) +{ + uint32_t tnow = AP_HAL::millis(); + _io_timer_heartbeat = tnow; + + if (!(_write_fd) || !_initialised || _open_error || !has_data) { + return; + } + + uint32_t nbytes = _writebuf.available(); + if (nbytes == 0) { + return; + } + if (nbytes < _writebuf_chunk && + tnow - _last_write_time < 2000UL) { + // write in _writebuf_chunk-sized chunks, but always write at + // least once per 2 seconds if data is available + has_data=false; + return; + } + + _last_write_time = tnow; + if (nbytes > _writebuf_chunk) { + // be kind to the FAT filesystem + nbytes = _writebuf_chunk; + } + + 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) { + nbytes -= ofs; + } + } + + if(nbytes==0) return; + + ssize_t nwritten = _write_fd.write(head, nbytes); + if (nwritten <= 0) { + FRESULT err=SD.lastError; + printf("\nLog write %ld bytes fails: %s\n",nbytes, SD.strError(err)); + gcs().send_text(MAV_SEVERITY_WARNING,"Log write %ld bytes fails: %s\n",nbytes, SD.strError(err)); +// stop_logging(); + _write_fd.close(); +#if defined(BOARD_DATAFLASH_FATFS) + if(FR_INT_ERR == err || FR_NO_FILESYSTEM == err) { // internal error - bad filesystem + gcs().send_text(MAV_SEVERITY_INFO, "Formatting DataFlash, please wait"); + uint32_t t=AP_HAL::millis(); + _busy = true; // format requires a long time and 1s task will kill process + SD.format(_log_directory); + _busy = false; + gcs().send_text(MAV_SEVERITY_INFO, "Formatting complete in %ldms", AP_HAL::millis() - t); + start_new_log(); // re-open logging + if(_write_fd) { // success? + nwritten = _write_fd.write(head, nbytes); // ok, try to write again + if(nwritten>0) { // if ok + _write_offset += nwritten; // then mark data as written + _writebuf.advance(nwritten); + _write_fd.sync(); // and fix it on SD + return; + } + } + } else +#endif + { + _busy = true; // Prep_MinSpace requires a long time and 1s task will kill process + Prep_MinSpace(); + _busy = false; + start_new_log(); // re-open logging + if(_write_fd) { // success? + nwritten = _write_fd.write(head, nbytes); // ok, try to write again + if(nwritten>0) { // if ok + _write_offset += nwritten; // then mark data as written + _writebuf.advance(nwritten); + _write_fd.sync(); // and fix it on SD + return; + } + } + } + + _initialised = false; + } else { + _write_offset += nwritten; + _writebuf.advance(nwritten); + /* + 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. + */ + _write_fd.sync(); + +#if defined(BOARD_DATAFLASH_FATFS) // limit file size in some MBytes and reopen new log file + + if(_write_fd.size() >= MAX_FILE_SIZE) { // size > 2M + stop_logging(); + uint32_t t = AP_HAL::millis(); + _busy = true; + Prep_MinSpace(); + _busy = false; + printf("\nlog file reopened in %ldms\n", AP_HAL::millis() - t); + start_new_log(); // re-start logging + } +#endif + } +} + +// this sensor is enabled if we should be logging at the moment +bool DataFlash_File::logging_enabled() const +{ + if (hal.util->get_soft_armed() || + _front.log_while_disarmed()) { + return true; + } + return false; +} + +bool DataFlash_File::io_thread_alive() const +{ + uint32_t tnow = AP_HAL::millis(); + // if the io thread hasn't had a heartbeat in a 5 second then it is dead + if(_io_timer_heartbeat + 5000 > tnow) return true; + + return false; +} + +bool DataFlash_File::logging_failed() const +{ + bool op=false; + + if(_write_fd) op=true; + + if (!op && + (hal.util->get_soft_armed() || + _front.log_while_disarmed())) { + return true; + } + if (_open_error) { + return true; + } + if (!io_thread_alive()) { + // No heartbeat in a second. IO thread is dead?! Very Not Good. + return true; + } + + return false; +} + + +void DataFlash_File::vehicle_was_disarmed() +{ + if (_front._params.file_disarm_rot) { + // rotate our log. Closing the current one and letting the + // logging restart naturally based on log_disarmed should do + // the trick: + stop_logging(); + } +} + + + +///////////////////////////////////////////////////////////////////// +// функция конвертации между UNIX-временем и обычным представлением в виде даты и времени суток +#define _TBIAS_DAYS ((70 * (uint32_t)365) + 17) +#define _TBIAS_SECS (_TBIAS_DAYS * (xtime_t)86400) +#define _TBIAS_YEAR 1900 +#define MONTAB(year) ((((year) & 03) || ((year) == 0)) ? mos : lmos) + +const uint16_t lmos[] = {0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; +const uint16_t mos[] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}; + +#define Daysto32(year, mon) (((year - 1) / 4) + MONTAB(year)[mon]) + +uint32_t DataFlash_File::to_timestamp(const struct tm *t) +{ /* convert time structure to scalar time */ + int32_t days; + uint32_t secs; + int32_t mon, year; + + /* Calculate number of days. */ + mon = t->tm_mon - 1; + year = t->tm_year - _TBIAS_YEAR; + days = Daysto32(year, mon) - 1; + days += 365 * year; + days += t->tm_mday; + days -= _TBIAS_DAYS; + + /* Calculate number of seconds. */ + secs = 3600 * t->tm_hour; + secs += 60 * t->tm_min; + secs += t->tm_sec; + + secs += (days * (uint32_t)86400); + + return (secs); +} +#endif diff --git a/libraries/DataFlash/DataFlash_File_sd.h b/libraries/DataFlash/DataFlash_File_sd.h new file mode 100644 index 0000000000..5b7d558e21 --- /dev/null +++ b/libraries/DataFlash/DataFlash_File_sd.h @@ -0,0 +1,155 @@ +/* + DataFlash logging - file oriented variant + + This uses posix file IO to create log files called logNN.dat in the + given directory + */ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT && (defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS)) + +#include +#include "DataFlash_Backend.h" + +#include + +class DataFlash_File : public DataFlash_Backend +{ +public: + // constructor + DataFlash_File(DataFlash_Class &front, + DFMessageWriter_DFLogStart *, + const char *log_directory); + + // initialisation + void Init() override; + bool CardInserted(void) const; + + // erase handling + void EraseAll(); + + // possibly time-consuming preparation handling: + bool NeedPrep(); + void Prep(); + + /* Write a block of data at current offset */ + bool _WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical); + uint32_t bufferspace_available(); + + // high level interface + uint16_t find_last_log() override; + void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page); + void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc); + int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data); + uint16_t get_num_logs() override; + uint16_t start_new_log(void) override; + void LogReadProcess(const uint16_t log_num, + uint16_t start_page, uint16_t end_page, + print_mode_fn print_mode, + AP_HAL::BetterStream *port); + void DumpPageInfo(AP_HAL::BetterStream *port); + void ShowDeviceInfo(AP_HAL::BetterStream *port); + void ListAvailableLogs(AP_HAL::BetterStream *port); + + void periodic_1Hz(const uint32_t now) override; + void periodic_fullrate(const uint32_t now); + + // this method is used when reporting system status over mavlink + bool logging_enabled() const; + bool logging_failed() const; + + void vehicle_was_disarmed() override; + + bool logging_started(void) const override { return !(!(_write_fd)); } + + void PrepForArming(); + +protected: + bool WritesOK() const override; + bool StartNewLogOK() const override; + +private: + File _write_fd; + File _read_fd; + uint16_t _read_fd_log_num; + uint32_t _read_offset; + uint32_t _write_offset; + volatile bool _open_error; + const char *_log_directory; + + uint32_t _io_timer_heartbeat; + bool io_thread_alive() const; + + uint16_t _cached_oldest_log; + uint16_t _last_oldest_log; + + /* + read a block + */ + bool ReadBlock(void *pkt, uint16_t size) override; + + uint16_t _log_num_from_list_entry(const uint16_t list_entry); + + // possibly time-consuming preparations handling + void Prep_MinSpace(); + uint16_t find_oldest_log(); + + bool file_exists(const char *filename) const; + bool log_exists(const uint16_t lognum) const; + + const float min_avail_space_percent = 10.0f; + + // write buffer + ByteBuffer _writebuf; + const uint16_t _writebuf_chunk; + uint32_t _last_write_time; + + /* construct a file name given a log number. Caller must free. */ + char *_log_file_name(const uint16_t log_num) const; + char *_lastlog_file_name() const; + uint32_t _get_log_size(const uint16_t log_num) const; + uint32_t _get_log_time(const uint16_t log_num) const; + + void stop_logging(void); + + void _io_timer(void); + + uint32_t critical_message_reserved_space() const { + // possibly make this a proportional to buffer size? + uint32_t ret = 1024; + if (ret > _writebuf.get_size()) { + // in this case you will only get critical messages + ret = _writebuf.get_size(); + } + return ret; + }; + uint32_t non_messagewriter_message_reserved_space() const { + // possibly make this a proportional to buffer size? + uint32_t ret = 1024; + 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! + ret = 0; + } + return ret; + }; + + float avail_space_percent(uint32_t *free = NULL); + + AP_HAL::Semaphore *semaphore; + + bool has_data; + +#define Daysto32(year, mon) (((year - 1) / 4) + MONTAB(year)[mon]) + +///////////////////////////////////////////////////////////////////// + + static uint32_t to_timestamp(const struct tm *t); + + bool _busy; +}; + +#endif \ No newline at end of file diff --git a/libraries/DataFlash/DataFlash_Revo.cpp b/libraries/DataFlash/DataFlash_Revo.cpp new file mode 100644 index 0000000000..16b0f23382 --- /dev/null +++ b/libraries/DataFlash/DataFlash_Revo.cpp @@ -0,0 +1,1092 @@ +/* + hacked up DataFlash library for Desktop support +*/ + + +#include + + +#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT && defined(BOARD_DATAFLASH_CS_PIN) && !defined(BOARD_DATAFLASH_FATFS) + +#include "DataFlash_Revo.h" + + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#pragma GCC diagnostic ignored "-Wunused-result" + + +extern const AP_HAL::HAL& hal; + +static uint8_t buffer[2][DF_PAGE_SIZE]; +static uint8_t cmd[4]; + +AP_HAL::OwnPtr DataFlash_Revo::_spi = nullptr; +AP_HAL::Semaphore *DataFlash_Revo::_spi_sem = nullptr; +bool DataFlash_Revo::log_write_started=false; +bool DataFlash_Revo::flash_died=false; + + +// 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 0x28122013L + +uint32_t DataFlash_Revo::bufferspace_available() +{ + // because DataFlash_Block devices are ring buffers, we *always* + // have room... + return df_NumPages * df_PageSize; +} + +// *** DATAFLASH PUBLIC FUNCTIONS *** +void DataFlash_Revo::StartWrite(uint16_t PageAdr) +{ + df_BufferIdx = 0; + df_BufferNum = 0; + df_PageAdr = PageAdr; + WaitReady(); +} + +void DataFlash_Revo::FinishWrite(void) +{ + // Write Buffer to flash, NO WAIT + BufferToPage(df_BufferNum, df_PageAdr, 0); + df_PageAdr++; + // If we reach the end of the memory, start from the beginning + if (df_PageAdr > df_NumPages) { + df_PageAdr = 1; + } + +// TODO: а что, стирать уже не надо??? + uint16_t block_num = df_PageAdr / (erase_size/DF_PAGE_SIZE); // number of erase block + uint16_t page_in_block = df_PageAdr % (erase_size/DF_PAGE_SIZE); // number of page in erase block + +// if(block_num != last_block_num){ + if(page_in_block==0 || df_PageAdr==1){ // начали писАть страницу - подготовим ее + PageErase(df_PageAdr); + last_block_num = block_num; + } + + // switch buffer + df_BufferNum ^= 1; + df_BufferIdx = 0; +} + +bool DataFlash_Revo::WritesOK() const +{ + if (!CardInserted()) { + return false; + } + if (!log_write_started) { + return false; + } + return true; +} + +bool DataFlash_Revo::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, + bool is_critical) +{ + // is_critical is ignored - we're a ring buffer and never run out + // of space. possibly if we do more complicated bandwidth + // limiting we can reservice bandwidth based on is_critical + if (!WritesOK()) { + return false; + } + + if (! WriteBlockCheckStartupMessages()) { + return false; + } + + while (size > 0) { + uint16_t n = df_PageSize - df_BufferIdx; + if (n > size) { + n = size; + } + + if (df_BufferIdx == 0) { + // if we are at the start of a page we need to insert a + // page header + if (n > df_PageSize - sizeof(struct PageHeader)) { + n = df_PageSize - sizeof(struct PageHeader); + } + struct PageHeader ph = { df_FileNumber, df_FilePage }; + BlockWrite(df_BufferNum, df_BufferIdx, &ph, sizeof(ph), pBuffer, n); + df_BufferIdx += n + sizeof(ph); + } else { + BlockWrite(df_BufferNum, df_BufferIdx, nullptr, 0, pBuffer, n); + df_BufferIdx += n; + } + + size -= n; + pBuffer = (const void *)(n + (uintptr_t)pBuffer); + + if (df_BufferIdx == df_PageSize) { + FinishWrite(); + df_FilePage++; + } + } + + return true; +} + + +// Get the last page written to +uint16_t DataFlash_Revo::GetWritePage() +{ + return df_PageAdr; +} + +// Get the last page read +uint16_t DataFlash_Revo::GetPage() +{ + return df_Read_PageAdr; +} + +void DataFlash_Revo::StartRead(uint16_t PageAdr) +{ + df_Read_BufferNum = 0; + df_Read_PageAdr = PageAdr; + + // disable writing while reading + log_write_started = false; + + WaitReady(); + + // copy flash page to buffer + PageToBuffer(df_Read_BufferNum, df_Read_PageAdr); + + // We are starting a new page - read FileNumber and FilePage + struct PageHeader ph; + BlockRead(df_Read_BufferNum, 0, &ph, sizeof(ph)); + df_FileNumber = ph.FileNumber; + df_FilePage = ph.FilePage; + df_Read_BufferIdx = sizeof(ph); +} + +bool DataFlash_Revo::ReadBlock(void *pBuffer, uint16_t size) +{ + while (size > 0) { + uint16_t n = df_PageSize - df_Read_BufferIdx; + if (n > size) { + n = size; + } + + WaitReady(); + + if (!BlockRead(df_Read_BufferNum, df_Read_BufferIdx, pBuffer, n)) { + return false; + } + size -= n; + pBuffer = (void *)(n + (uintptr_t)pBuffer); + + df_Read_BufferIdx += n; + + if (df_Read_BufferIdx == df_PageSize) { + df_Read_PageAdr++; + if (df_Read_PageAdr > df_NumPages) { + df_Read_PageAdr = 1; + } + PageToBuffer(df_Read_BufferNum, df_Read_PageAdr); + + // We are starting a new page - read FileNumber and FilePage + struct PageHeader ph; + if (!BlockRead(df_Read_BufferNum, 0, &ph, sizeof(ph))) { + return false; + } + df_FileNumber = ph.FileNumber; + df_FilePage = ph.FilePage; + + df_Read_BufferIdx = sizeof(ph); + } + } + return true; +} + +void DataFlash_Revo::SetFileNumber(uint16_t FileNumber) +{ + df_FileNumber = FileNumber; + df_FilePage = 1; +} + +uint16_t DataFlash_Revo::GetFileNumber() +{ + return df_FileNumber; +} + +uint16_t DataFlash_Revo::GetFilePage() +{ + return df_FilePage; +} + +void DataFlash_Revo::EraseAll() +{ + log_write_started = false; + + ChipErase(); + + // write the logging format in the last page + hal.scheduler->delay(100); + StartWrite(df_NumPages+1); + uint32_t version = DF_LOGGING_FORMAT; + log_write_started = true; + BlockWrite(df_BufferNum, 0, nullptr, 0, &version, sizeof(version)); + + log_write_started = false; + FinishWrite(); + hal.scheduler->delay(100); + +//[ just to test + StartRead(df_NumPages+1); // read last page after erase to check it + + StartRead(1); +//] +} + +bool DataFlash_Revo::NeedPrep(void) +{ + return NeedErase(); +} + +void DataFlash_Revo::Prep() +{ + if (hal.util->get_soft_armed()) { + // do not want to do any filesystem operations while we are e.g. flying + return; + } + if (NeedErase()) { + EraseAll(); + } +} + +/* + * we need to erase if the logging format has changed + */ +bool DataFlash_Revo::NeedErase(void) +{ + uint32_t version = 0; + StartRead(df_NumPages+1); // last page + + BlockRead(df_Read_BufferNum, 0, &version, sizeof(version)); + StartRead(1); + if(version == DF_LOGGING_FORMAT) return false; + + printf("Need to erase: version is %lx required %lx\n", version, DF_LOGGING_FORMAT); + + return true; +} + +/** + get raw data from a log + */ +int16_t DataFlash_Revo::get_log_data_raw(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) +{ + uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader); + + if (offset >= data_page_size) { + page += offset / data_page_size; + offset = offset % data_page_size; + if (page > df_NumPages) { + // pages are one based, not zero + page = 1 + page - df_NumPages; + } + } + if (log_write_started || df_Read_PageAdr != page) { + StartRead(page); + } + + df_Read_BufferIdx = offset + sizeof(struct PageHeader); + if (!ReadBlock(data, len)) { + return -1; + } + + return (int16_t)len; +} + +/** + get data from a log, accounting for adding FMT headers + */ +int16_t DataFlash_Revo::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) +{ + if (offset == 0) { + uint8_t header[3]; + get_log_data_raw(log_num, page, 0, 3, header); + adding_fmt_headers = (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2 || header[2] != LOG_FORMAT_MSG); + } + uint16_t ret = 0; + + if (adding_fmt_headers) { + // the log doesn't start with a FMT message, we need to add + // them + const uint16_t fmt_header_size = num_types() * sizeof(struct log_Format); + while (offset < fmt_header_size && len > 0) { + struct log_Format pkt; + uint8_t t = offset / sizeof(pkt); + uint8_t ofs = offset % sizeof(pkt); + Log_Fill_Format(structure(t), pkt); + uint8_t n = sizeof(pkt) - ofs; + if (n > len) { + n = len; + } + memcpy(data, ofs + (uint8_t *)&pkt, n); + data += n; + offset += n; + len -= n; + ret += n; + } + offset -= fmt_header_size; + } + + if (len > 0) { + ret += get_log_data_raw(log_num, page, offset, len, data); + } + + return ret; +} + + +// Public Methods ////////////////////////////////////////////////////////////// +void DataFlash_Revo::Init() +{ + + df_NumPages=0; + +#if BOARD_DATAFLASH_ERASE_SIZE >= 65536 + erase_cmd=JEDEC_PAGE_ERASE; +#else + erase_cmd=JEDEC_SECTOR_ERASE; +#endif + erase_size = BOARD_DATAFLASH_ERASE_SIZE; + + GPIO::_pinMode(DF_RESET,OUTPUT); + GPIO::_setSpeed(DF_RESET, GPIO_speed_100MHz); + // Reset the chip + GPIO::_write(DF_RESET,0); + Scheduler::_delay(1); + GPIO::_write(DF_RESET,1); + + _spi = hal.spi->get_device(HAL_DATAFLASH_NAME); + + if (!_spi) { + AP_HAL::panic("PANIC: DataFlash SPIDeviceDriver not found"); + return; /* never reached */ + } + + _spi_sem = _spi->get_semaphore(); + if (!_spi_sem) { + AP_HAL::panic("PANIC: DataFlash SPIDeviceDriver semaphore is null"); + return; /* never reached */ + } + + + + _spi_sem->take(10); + _spi->set_speed(AP_HAL::Device::SPEED_LOW); + + DataFlash_Backend::Init(); + + _spi_sem->give(); + + df_NumPages = BOARD_DATAFLASH_PAGES - 1; // reserve last page for config information + + ReadManufacturerID(); + + getSectorCount(&df_NumPages); + + flash_died=false; + + log_write_started = true; + + df_PageSize = DF_PAGE_SIZE; + + + +} + +void DataFlash_Revo::WaitReady() { + if(flash_died) return; + + uint32_t t = AP_HAL::millis(); + while(ReadStatus()!=0){ + + Scheduler::yield(0); // пока ждем пусть другие работают + + if(AP_HAL::millis() - t > 4000) { + flash_died = true; + return; + } + } +} + +// try to take a semaphore safely from both in a timer and outside +bool DataFlash_Revo::_sem_take(uint8_t timeout) +{ + + if(!_spi_sem || flash_died) return false; + + return _spi_sem->take(timeout); +} + +bool DataFlash_Revo::cs_assert(){ + if (!_sem_take(HAL_SEMAPHORE_BLOCK_FOREVER)) + return false; + + _spi->set_speed(AP_HAL::Device::SPEED_HIGH); + + GPIO::_write(DF_RESET,0); + return true; +} + +void DataFlash_Revo::cs_release(){ + GPIO::_write(DF_RESET,1); + + _spi_sem->give(); +} + + +// This function is mainly to test the device +void DataFlash_Revo::ReadManufacturerID() +{ + if (!cs_assert()) return; + + // Read manufacturer and ID command... +#if 0 + spi_write(JEDEC_DEVICE_ID); // + + df_manufacturer = spi_read(); + df_device = spi_read(); //memorytype + df_device = (df_device << 8) | spi_read(); //capacity + spi_read(); // ignore 4th byte +#else + cmd[0] = JEDEC_DEVICE_ID; + + _spi->transfer(cmd, 1, buffer[1], 4); + + df_manufacturer = buffer[1][0]; + df_device = (buffer[1][1] << 8) | buffer[1][2]; //capacity +#endif + + cs_release(); +} + +bool DataFlash_Revo::getSectorCount(uint32_t *ptr){ + uint8_t capacity = df_device & 0xFF; + uint8_t memtype = (df_device>>8) & 0xFF; + uint32_t size=0; + + + const char * mfg=NULL; + + switch(df_manufacturer){ + case 0xEF: // Winbond Serial Flash + if (memtype == 0x40) { + mfg="Winbond"; + size = (1 << ((capacity & 0x0f) + 8)); +/* + const uint8_t _capID[11] = {0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x43}; + const uint32_t _memSize[11] = {64L*K, 128L*K, 256L*K, 512L*K, 1L*M, 2L*M, 4L*M, 8L*M, 16L*M, 32L*M, 8L*M}; +*/ + erase_size=4096; + erase_cmd=JEDEC_SECTOR_ERASE; + } + break; + case 0xbf: // SST + if (memtype == 0x25) { + mfg="Microchip"; + size = (1 << ((capacity & 0x07) + 12)); + } + break; + + case 0x20: // micron + if (memtype == 0xba){// JEDEC_ID_MICRON_N25Q128 0x20ba18 + mfg="Micron"; + size = (1 << ((capacity & 0x0f) + 8)); + erase_size=4096; + erase_cmd=JEDEC_SECTOR_ERASE; + } else if(memtype==0x20) { // JEDEC_ID_MICRON_M25P16 0x202015 + mfg="Micron"; + size = (1 << ((capacity & 0x0f) + 8)); + } + break; + + case 0xC2: //JEDEC_ID_MACRONIX_MX25L6406E 0xC22017 + if (memtype == 0x20) { + mfg="MACRONIX"; + size = (1 << ((capacity & 0x0f) + 8)); + erase_size=4096; + erase_cmd=JEDEC_SECTOR_ERASE; + } + break; + + case 0x9D: // ISSI + if (memtype == 0x40 || memtype == 0x30) { + mfg = "ISSI"; + size = (1 << ((capacity & 0x0f) + 8)); + } + break; + + default: + break; + } + + if(mfg && size) { + printf("%s SPI Flash found sectors=%ld\n", mfg, size); + }else { + printf("\nUnknown Flash! SPI Flash codes: mfg=%x type=%x cap=%x\n ",df_manufacturer, memtype, capacity); + size = BOARD_DATAFLASH_PAGES; // as defined + } + + +/////// + + size -= (erase_size/DF_PAGE_SIZE); // reserve last page for config information + + *ptr = size; // in 256b blocks + + return true; + +} + +// Read the status register +uint8_t DataFlash_Revo::ReadStatusReg() +{ + uint8_t tmp; + + // activate dataflash command decoder + if (!cs_assert()) return JEDEC_STATUS_BUSY; + + // Read status command +#if 0 + spi_write(JEDEC_READ_STATUS); + tmp = spi_read(); // We only want to extract the READY/BUSY bit +#else + cmd[0] = JEDEC_READ_STATUS; + + _spi->transfer(cmd, 1, &cmd[1], 1); + tmp = cmd[1]; +#endif + // release SPI bus for use by other sensors + cs_release(); + + return tmp; +} + +uint8_t DataFlash_Revo::ReadStatus() +{ + // We only want to extract the READY/BUSY bit + int32_t status = ReadStatusReg(); + if (status < 0) + return -1; + return status & JEDEC_STATUS_BUSY; +} + + +void DataFlash_Revo::PageToBuffer(unsigned char BufferNum, uint16_t pageNum) +{ + + uint32_t PageAdr = pageNum * DF_PAGE_SIZE; + + if (!cs_assert()) return; + + cmd[0] = JEDEC_READ_DATA; + cmd[1] = (PageAdr >> 16) & 0xff; + cmd[2] = (PageAdr >> 8) & 0xff; + cmd[3] = (PageAdr >> 0) & 0xff; + + _spi->transfer(cmd, 4, buffer[BufferNum], DF_PAGE_SIZE); + + cs_release(); +} + +void DataFlash_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum, unsigned char wait) +{ + uint32_t PageAdr = pageNum * DF_PAGE_SIZE; + + Flash_Jedec_WriteEnable(); + + if (!cs_assert()) return; + + cmd[0] = JEDEC_PAGE_WRITE; + cmd[1] = (PageAdr >> 16) & 0xff; + cmd[2] = (PageAdr >> 8) & 0xff; + cmd[3] = (PageAdr >> 0) & 0xff; + + _spi->transfer(cmd, 4,NULL, 0); + + _spi->transfer(buffer[BufferNum], DF_PAGE_SIZE, NULL, 0); + + cs_release(); + + if(wait) WaitReady(); + +} + +void DataFlash_Revo::BufferWrite (unsigned char BufferNum, uint16_t IntPageAdr, unsigned char Data) +{ + buffer[BufferNum][IntPageAdr] = (uint8_t)Data; +} + +void DataFlash_Revo::BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr, + const void *pHeader, uint8_t hdr_size, + const void *pBuffer, uint16_t size) +{ + if (hdr_size) { + memcpy(&buffer[BufferNum][IntPageAdr], + pHeader, + hdr_size); + } + memcpy(&buffer[BufferNum][IntPageAdr+hdr_size], + pBuffer, + size); +} + +// read size bytes of data to a page. The caller must ensure that +// the data fits within the page, otherwise it will wrap to the +// start of the page +bool DataFlash_Revo::BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size) +{ + memcpy(pBuffer, &buffer[BufferNum][IntPageAdr], size); + return true; +} + +/* + +* 2 097 152 bytes (8 bits each) +* 32 sectors (512 Kbits, 65536 bytes each) +* 8192 pages (256 bytes each). + +*/ + +void DataFlash_Revo::PageErase (uint16_t pageNum) +{ + + uint32_t PageAdr = pageNum * DF_PAGE_SIZE; + + cmd[0] = erase_cmd; + cmd[1] = (PageAdr >> 16) & 0xff; + cmd[2] = (PageAdr >> 8) & 0xff; + cmd[3] = (PageAdr >> 0) & 0xff; + + Flash_Jedec_WriteEnable(); + + if (!cs_assert()) return; + + _spi->transfer(cmd, 4, NULL, 0); + + cs_release(); +} + + +void DataFlash_Revo::ChipErase() +{ + + cmd[0] = JEDEC_BULK_ERASE; + + Flash_Jedec_WriteEnable(); + + if (!cs_assert()) return; + + _spi->transfer(cmd, 1, NULL, 0); + + cs_release(); +} + + +void DataFlash_Revo::Flash_Jedec_WriteEnable(void) +{ + // activate dataflash command decoder + if (!cs_assert()) return; + + spi_write(JEDEC_WRITE_ENABLE); + + cs_release(); +} + +////////////////////////////////////////// + +// This function determines the number of whole or partial log files in the DataFlash +// Wholly overwritten files are (of course) lost. +uint16_t DataFlash_Revo::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); + if (GetFileNumber() == 0xFFFF) + StartRead(((lastpage >> 8) + 1) << 8); // next sector + first = GetFileNumber(); + if(first > last) { + StartRead(1); + first = GetFileNumber(); + } + + if (last == first) { + return 1; + } + + return (last - first + 1); +} + + +// This function starts a new log file in the DataFlash +uint16_t DataFlash_Revo::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"); + log_write_started = true; + return 1; + } + + uint16_t new_log_num; + + // Check for log of length 1 page and suppress + if(GetFilePage() <= 1) { + new_log_num = GetFileNumber(); + // Last log too short, reuse its number + // and overwrite it + SetFileNumber(new_log_num); + StartWrite(last_page); + } else { + new_log_num = GetFileNumber()+1; + if (last_page == 0xFFFF) { + last_page=0; + } + SetFileNumber(new_log_num); + StartWrite(last_page + 1); + } + log_write_started = true; + return new_log_num; +} + +// 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_Revo::get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) +{ + uint16_t num = get_num_logs(); + uint16_t look; + + if (df_BufferIdx != 0) { + FinishWrite(); + hal.scheduler->delay(100); + } + + 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 == df_NumPages+1 || start_page == 0) { + start_page = 1; + } + end_page = find_last_page_of_log(log_num); + if (end_page == 0) { + end_page = start_page; + } + + +} + +bool DataFlash_Revo::check_wrapped(void) +{ + StartRead(df_NumPages); + if(GetFileNumber() == 0xFFFF) + return 0; + else + return 1; +} + + +// This funciton finds the last log number +uint16_t DataFlash_Revo::find_last_log(void) +{ + uint16_t last_page = find_last_page(); + StartRead(last_page); + return GetFileNumber(); +} + +// This function finds the last page of the last file +uint16_t DataFlash_Revo::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 = ((int32_t)GetFileNumber()<<16) | GetFilePage(); + + while(top-bottom > 1) { + look = (top+bottom)/2; + StartRead(look); + look_hash = (int32_t)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 = ((int32_t)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 +uint16_t DataFlash_Revo::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 = (int32_t)log_number<<16 | 0xFFFF; + + while(top-bottom > 1) + { + look = (top+bottom)/2; + StartRead(look); + look_hash = (int32_t)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; +} + + + + + +/* + dump header information from all log pages + */ +void DataFlash_Revo::DumpPageInfo(AP_HAL::BetterStream *port) +{ + for (uint16_t count=1; count<=df_NumPages; count++) { + StartRead(count); + port->printf("DF page, log file #, log page: %u,\t", (unsigned)count); + port->printf("%u,\t", (unsigned)GetFileNumber()); + port->printf("%u\n", (unsigned)GetFilePage()); + } +} + +/* + show information about the device + */ +void DataFlash_Revo::ShowDeviceInfo(AP_HAL::BetterStream *port) +{ + if (!CardInserted()) { + port->printf("No dataflash inserted\n"); + return; + } + ReadManufacturerID(); + port->printf("Manufacturer: 0x%02x Device: 0x%04x\n", + (unsigned)df_manufacturer, + (unsigned)df_device); + port->printf("NumPages: %u PageSize: %u\n", + (unsigned)df_NumPages+1, + (unsigned)df_PageSize); +} + +/* + list available log numbers + */ +void DataFlash_Revo::ListAvailableLogs(AP_HAL::BetterStream *port) +{ + uint16_t num_logs = get_num_logs(); + int16_t last_log_num = find_last_log(); + uint16_t log_start = 0; + uint16_t log_end = 0; + + if (num_logs == 0) { + port->printf("\nNo logs\n\n"); + return; + } + port->printf("\n%u logs\n", (unsigned)num_logs); + + for (uint16_t i=num_logs; i>=1; i--) { + uint16_t last_log_start = log_start, last_log_end = log_end; + uint16_t temp = last_log_num - i + 1; + get_log_boundaries(temp, log_start, log_end); + port->printf("Log %u, start %u, end %u\n", + (unsigned)temp, + (unsigned)log_start, + (unsigned)log_end); + if (last_log_start == log_start && last_log_end == log_end) { + // we are printing bogus logs + break; + } + } + port->println(); +} + +/* + Read the log and print it on port +*/ +void DataFlash_Revo::LogReadProcess(uint16_t log_num, + uint16_t start_page, uint16_t end_page, + print_mode_fn print_mode, + AP_HAL::BetterStream *port) +{ + uint8_t log_step = 0; + uint16_t page = start_page; +// bool first_entry = true; + + if (df_BufferIdx != 0) { + FinishWrite(); + hal.scheduler->delay(100); + } + + StartRead(start_page); + + while (true) { + uint8_t data; + ReadBlock(&data, 1); + + // 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; +// first_entry = false; + _print_log_entry(data, print_mode, port); + break; + } + uint16_t new_page = GetPage(); + if (new_page != page) { + if (new_page == end_page+1 || new_page == start_page) { + return; + } + page = new_page; + } + } +} + +void DataFlash_Revo::get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) +{ + uint16_t start, end; + get_log_boundaries(log_num, start, end); + if (end >= start) { + size = (end + 1 - start) * (uint32_t)df_PageSize; + } else { + size = (df_NumPages + end - start) * (uint32_t)df_PageSize; + } + time_utc = 0; +} + +#endif diff --git a/libraries/DataFlash/DataFlash_Revo.h b/libraries/DataFlash/DataFlash_Revo.h new file mode 100644 index 0000000000..c6b32d3d6e --- /dev/null +++ b/libraries/DataFlash/DataFlash_Revo.h @@ -0,0 +1,201 @@ +/* ************************************************************ */ +/* DataFlash_Revo Log library */ +/* ************************************************************ */ +#pragma once + +#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT + +#include +#include "DataFlash_Backend.h" +#include +#include + + +// flash size +#define DF_PAGE_SIZE 256L + +#define DF_RESET BOARD_DATAFLASH_CS_PIN // RESET (PB3) + +//Micron M25P16 Serial Flash Embedded Memory 16 Mb, 3V +#define JEDEC_WRITE_ENABLE 0x06 +#define JEDEC_WRITE_DISABLE 0x04 +#define JEDEC_READ_STATUS 0x05 +#define JEDEC_WRITE_STATUS 0x01 +#define JEDEC_READ_DATA 0x03 +#define JEDEC_FAST_READ 0x0b +#define JEDEC_DEVICE_ID 0x9F +#define JEDEC_PAGE_WRITE 0x02 + +#define JEDEC_BULK_ERASE 0xC7 +#define JEDEC_SECTOR_ERASE 0x20 // 4k erase +#define JEDEC_PAGE_ERASE 0xD8 // 64K erase + +#define JEDEC_STATUS_BUSY 0x01 +#define JEDEC_STATUS_WRITEPROTECT 0x02 +#define JEDEC_STATUS_BP0 0x04 +#define JEDEC_STATUS_BP1 0x08 +#define JEDEC_STATUS_BP2 0x10 +#define JEDEC_STATUS_TP 0x20 +#define JEDEC_STATUS_SEC 0x40 +#define JEDEC_STATUS_SRP0 0x80 + + +using namespace F4Light; + + +class DataFlash_Revo : public DataFlash_Backend +{ +private: + //Methods + void BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data); + void BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait); + void PageToBuffer(uint8_t BufferNum, uint16_t PageAdr); + void WaitReady(); + uint8_t ReadStatusReg(); + uint16_t PageSize() { return df_PageSize; } + void PageErase (uint16_t PageAdr); + void BlockErase (uint16_t BlockAdr); + void ChipErase(); + + void Flash_Jedec_WriteEnable(); + void Flash_Jedec_EraseSector(uint32_t chip_offset); + + // write size bytes of data to a page. The caller must ensure that + // the data fits within the page, otherwise it will wrap to the + // start of the page + // If pHeader is not nullptr then write the header bytes before the data + void BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr, + const void *pHeader, uint8_t hdr_size, + const void *pBuffer, uint16_t size); + + // read size bytes of data to a page. The caller must ensure that + // the data fits within the page, otherwise it will wrap to the + // start of the page + bool BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size); + + + ////////////////// + static AP_HAL::OwnPtr _spi; + static AP_HAL::Semaphore *_spi_sem; + static bool log_write_started; + + static bool _sem_take(uint8_t timeout); // take a semaphore safely + + bool cs_assert(); // Select device + void cs_release(); // Deselect device + +// uint8_t spi_read(void) { uint8_t b; _spi->transfer(NULL,0, &b, 1); return b; } + inline void spi_write(uint8_t b) { _spi->transfer(&b,1, NULL, 0); } + inline void spi_write(int data) { spi_write((uint8_t)data); } + + static bool flash_died; + +//[ from died Dataflash_Block + + struct PageHeader { + uint16_t FileNumber; + uint16_t FilePage; + }; + + // DataFlash Log variables... + uint8_t df_BufferNum; + uint8_t df_Read_BufferNum; + uint16_t df_BufferIdx; + uint16_t df_Read_BufferIdx; + uint16_t df_PageAdr; + uint16_t df_Read_PageAdr; + uint16_t df_FileNumber; + uint16_t df_FilePage; + + // offset from adding FMT messages to log data + bool adding_fmt_headers; + + // erase handling + bool NeedErase(void); + + // internal high level functions + void StartRead(uint16_t PageAdr); + uint16_t find_last_page(void); + uint16_t find_last_page_of_log(uint16_t log_number); + bool check_wrapped(void); + uint16_t GetPage(void); + uint16_t GetWritePage(void); + void StartWrite(uint16_t PageAdr); + void FinishWrite(void); + bool getSectorCount(uint32_t *ptr); + + // Read methods + bool ReadBlock(void *pBuffer, uint16_t size); + + // file numbers + void SetFileNumber(uint16_t FileNumber); + uint16_t GetFilePage(); + uint16_t GetFileNumber(); + + uint8_t erase_cmd; + uint32_t erase_size; + uint16_t last_block_num; + +protected: + uint8_t df_manufacturer; + uint16_t df_device; + + // page handling + uint16_t df_PageSize; + uint32_t df_NumPages; + + bool WritesOK() const override; + +//] + + +public: + DataFlash_Revo(DataFlash_Class &front, DFMessageWriter_DFLogStart *writer) : + DataFlash_Backend(front, writer) { } + + void Init() override; + void ReadManufacturerID(); + bool CardInserted(void) const { return true; } + + uint8_t ReadStatus(); + + bool logging_enabled() const { return true; } + bool logging_failed() const { return false; }; + + void stop_logging(void) { log_write_started = false; } + +//[ from died Dataflash_Block + + // erase handling + void EraseAll(); + + bool NeedPrep(void); + void Prep(); + + /* Write a block of data at current offset */ + bool _WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical); + + // high level interface + uint16_t get_num_logs() override; + uint16_t start_new_log(void); + void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page); + uint16_t find_last_log() override; + void LogReadProcess(const uint16_t list_entry, + uint16_t start_page, uint16_t end_page, + print_mode_fn print_mode, + AP_HAL::BetterStream *port); + void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc); + void DumpPageInfo(AP_HAL::BetterStream *port); + void ShowDeviceInfo(AP_HAL::BetterStream *port); + void ListAvailableLogs(AP_HAL::BetterStream *port); + + + int16_t get_log_data_raw(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data); + int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data); + + uint32_t bufferspace_available(); + + bool logging_started(void) const { return log_write_started; } +}; + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_Revo diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 94006df856..b1f1c2700a 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -14,7 +14,10 @@ #include "DataFlash.h" #include "DataFlash_File.h" +#include "DataFlash_File_sd.h" #include "DataFlash_MAVLink.h" +#include "DataFlash_Revo.h" +#include "DataFlash_File_sd.h" #include "DFMessageWriter.h" extern const AP_HAL::HAL& hal;