diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index bb6f8c81a1..c9f90ed4a5 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -90,8 +90,8 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = { // @Param: _DISARMED // @DisplayName: Enable logging while disarmed - // @Description: If LOG_DISARMED is set to 1 then logging will be enabled at all times including when disarmed. Logging before arming can make for very large logfiles but can help a lot when tracking down startup issues and is necessary if logging of EKF replay data is selected via the LOG_REPLAY parameter. If LOG_DISARMED is set to 2, then logging will be enabled when disarmed, but not if a USB connection is detected. This can be used to prevent unwanted data logs being generated when the vehicle is connected via USB for log downloading or parameter changes. - // @Values: 0:Disabled,1:Enabled,2:Disabled on USB connection + // @Description: If LOG_DISARMED is set to 1 then logging will be enabled at all times including when disarmed. Logging before arming can make for very large logfiles but can help a lot when tracking down startup issues and is necessary if logging of EKF replay data is selected via the LOG_REPLAY parameter. If LOG_DISARMED is set to 2, then logging will be enabled when disarmed, but not if a USB connection is detected. This can be used to prevent unwanted data logs being generated when the vehicle is connected via USB for log downloading or parameter changes. If LOG_DISARMED is set to 3 then logging will happen while disarmed, but if the vehicle never arms then the logs using the filesystem backend will be discarded on the next boot. + // @Values: 0:Disabled,1:Enabled,2:Disabled on USB connection,3:Discard log on reboot if never armed // @User: Standard AP_GROUPINFO("_DISARMED", 2, AP_Logger, _params.log_disarmed, 0), @@ -1469,6 +1469,7 @@ bool AP_Logger::log_while_disarmed(void) const return true; } if (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED || + _params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_DISCARD || (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_NOT_USB && !hal.gpio->usb_connected())) { return true; } diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index b4fc3e93d3..21715f7cc2 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -319,6 +319,7 @@ public: NONE = 0, LOG_WHILE_DISARMED = 1, LOG_WHILE_DISARMED_NOT_USB = 2, + LOG_WHILE_DISARMED_DISCARD = 3, }; // parameter support diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 7ee5c18ee5..351cf2f6bf 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -94,6 +94,16 @@ void AP_Logger_File::Init() _log_directory = custom_dir; } + uint16_t last_log_num = find_last_log(); + if (last_log_is_marked_discard) { + // delete the last log leftover from LOG_DISARMED=3 + char *filename = _log_file_name(last_log_num); + if (filename != nullptr) { + AP::FS().unlink(filename); + free(filename); + } + } + Prep_MinSpace(); } @@ -518,8 +528,13 @@ uint16_t AP_Logger_File::find_last_log() EXPECT_DELAY_MS(3000); FileData *fd = AP::FS().load_file(fname); free(fname); + last_log_is_marked_discard = false; if (fd != nullptr) { - ret = strtol((const char *)fd->data, nullptr, 10); + char *endptr = nullptr; + ret = strtol((const char *)fd->data, &endptr, 10); + if (endptr != nullptr) { + last_log_is_marked_discard = *endptr == 'D'; + } delete fd; } return ret; @@ -846,6 +861,18 @@ void AP_Logger_File::start_new_log(void) _writebuf.clear(); write_fd_semaphore.give(); + // now update lastlog.txt with the new log number + last_log_is_marked_discard = _front._params.log_disarmed == AP_Logger::LogDisarmed::LOG_WHILE_DISARMED_DISCARD; + if (!write_lastlog_file(log_num)) { + _open_error_ms = AP_HAL::millis(); + } +} + +/* + write LASTLOG.TXT, possibly with a discard marker + */ +bool AP_Logger_File::write_lastlog_file(uint16_t log_num) +{ // now update lastlog.txt with the new log number char *fname = _lastlog_file_name(); @@ -853,25 +880,17 @@ void AP_Logger_File::start_new_log(void) int fd = AP::FS().open(fname, O_WRONLY|O_CREAT); free(fname); if (fd == -1) { - _open_error_ms = AP_HAL::millis(); - return; + return false; } char buf[30]; - snprintf(buf, sizeof(buf), "%u\r\n", (unsigned)log_num); + snprintf(buf, sizeof(buf), "%u%s\r\n", (unsigned)log_num, last_log_is_marked_discard?"D":""); const ssize_t to_write = strlen(buf); const ssize_t written = AP::FS().write(fd, buf, to_write); AP::FS().close(fd); - - if (written < to_write) { - _open_error_ms = AP_HAL::millis(); - return; - } - - return; + return written == to_write; } - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX void AP_Logger_File::flush(void) #if APM_BUILD_TYPE(APM_BUILD_Replay) @@ -921,6 +940,13 @@ void AP_Logger_File::io_timer(void) return; } + if (last_log_is_marked_discard && hal.util->get_soft_armed()) { + // time to make the log permanent + const auto log_num = find_last_log(); + last_log_is_marked_discard = false; + write_lastlog_file(log_num); + } + uint32_t nbytes = _writebuf.available(); if (nbytes == 0) { return; diff --git a/libraries/AP_Logger/AP_Logger_File.h b/libraries/AP_Logger/AP_Logger_File.h index 7d1f6496c2..dbfe61f807 100644 --- a/libraries/AP_Logger/AP_Logger_File.h +++ b/libraries/AP_Logger/AP_Logger_File.h @@ -70,6 +70,7 @@ protected: private: int _write_fd = -1; char *_write_filename; + bool last_log_is_marked_discard; uint32_t _last_write_ms; #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS bool _need_rtc_update; @@ -101,6 +102,7 @@ private: bool log_exists(const uint16_t lognum) const; bool dirent_to_log_num(const dirent *de, uint16_t &log_num) const; + bool write_lastlog_file(uint16_t log_num); // write buffer ByteBuffer _writebuf{0};