diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index bc1b10e30b..e91f7ce66f 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -103,7 +103,7 @@ void Storage::_storage_open(void) } // pre-fill to full size if (AP::FS().lseek(log_fd, ret, SEEK_SET) != ret || - AP::FS().write(log_fd, &_buffer[ret], CH_STORAGE_SIZE-ret) != CH_STORAGE_SIZE-ret) { + (CH_STORAGE_SIZE-ret > 0 && AP::FS().write(log_fd, &_buffer[ret], CH_STORAGE_SIZE-ret) != CH_STORAGE_SIZE-ret)) { ::printf("setup failed for " HAL_STORAGE_FILE "\n"); AP::FS().close(log_fd); log_fd = -1; diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 1a3bbcccf5..0efe6e478f 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -542,6 +542,19 @@ bool AP_Logger::should_log(const uint32_t mask) const return true; } +/* + return true if in log download which should prevent logging + */ +bool AP_Logger::in_log_download() const +{ + if (uint8_t(_params.backend_types) & uint8_t(Backend_Type::BLOCK)) { + // when we have a BLOCK backend then listing completely prevents logging + return transfer_activity != transfer_activity_t::IDLE; + } + // for other backends listing does not interfere with logging + return transfer_activity == transfer_activity_t::SENDING; +} + const struct UnitStructure *AP_Logger::unit(uint16_t num) const { return &_units[num]; diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 3e2741f81c..fa5a6ef479 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -364,7 +364,7 @@ class AP_Logger bool vehicle_is_armed() const { return _armed; } void handle_log_send(); - bool in_log_download() const { return transfer_activity != IDLE; } + bool in_log_download() const; float quiet_nanf() const { return nanf("0x4152"); } // "AR" double quiet_nan() const { return nan("0x4152445550490a"); } // "ARDUPI" @@ -488,6 +488,9 @@ class AP_Logger SENDING, // actively sending log_sending packets } transfer_activity = IDLE; + // last time we handled a log-transfer-over-mavlink message: + uint32_t _last_mavlink_log_transfer_message_handled_ms; + // next log list entry to send uint16_t _log_next_list_entry; diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 1c93d62025..96abb83337 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -381,6 +381,18 @@ bool AP_Logger_Backend::ShouldLog(bool is_critical) return false; } + if (_front.in_log_download() && + _front._last_mavlink_log_transfer_message_handled_ms != 0) { + if (AP_HAL::millis() - _front._last_mavlink_log_transfer_message_handled_ms < 10000) { + if (!_front.vehicle_is_armed()) { + // user is transfering files via mavlink + return false; + } + } else { + _front._last_mavlink_log_transfer_message_handled_ms = 0; + } + } + if (is_critical && have_logged_armed && !_front._params.file_disarm_rot) { // if we have previously logged while armed then we log all // critical messages from then on. That fixes a problem where diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 06992441c6..f84d0fa10c 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -792,6 +792,10 @@ void AP_Logger_File::stop_logging(void) void AP_Logger_File::PrepForArming() { + if (_rotate_pending) { + _rotate_pending = false; + stop_logging(); + } if (logging_started() && _front._params.file_disarm_rot == 0) { return; diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index 7a93f859f8..585099a989 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -45,6 +45,7 @@ void AP_Logger::handle_log_message(GCS_MAVLINK &link, const mavlink_message_t &m if (!should_handle_log_message()) { return; } + _last_mavlink_log_transfer_message_handled_ms = AP_HAL::millis(); switch (msg.msgid) { case MAVLINK_MSG_ID_LOG_REQUEST_LIST: handle_log_request_list(link, msg); diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index a27de642f0..49c18ba761 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -1,10 +1,13 @@ #include "AP_Common/AP_FWVersion.h" #include "LoggerMessageWriter.h" +#include #define FORCE_VERSION_H_INCLUDE #include "ap_version.h" #undef FORCE_VERSION_H_INCLUDE +#define MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US 200 + extern const AP_HAL::HAL& hal; /* LogStartup - these are simple state machines which allow us to @@ -39,6 +42,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::FORMATS: // write log formats so the log is self-describing while (next_format_to_send < _logger_backend->num_types()) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send))) { return; // call me again! } @@ -50,6 +56,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::UNITS: while (_next_unit_to_send < _logger_backend->num_units()) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_logger_backend->Write_Unit(_logger_backend->unit(_next_unit_to_send))) { return; // call me again! } @@ -60,6 +69,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::MULTIPLIERS: while (_next_multiplier_to_send < _logger_backend->num_multipliers()) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_logger_backend->Write_Multiplier(_logger_backend->multiplier(_next_multiplier_to_send))) { return; // call me again! } @@ -70,6 +82,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::FORMAT_UNITS: while (_next_format_unit_to_send < _logger_backend->num_types()) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_logger_backend->Write_Format_Units(_logger_backend->structure(_next_format_unit_to_send))) { return; // call me again! } @@ -80,6 +95,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::PARMS: while (ap) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_logger_backend->Write_Parameter(ap, token, type)) { return; } @@ -91,6 +109,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::SYSINFO: _writesysinfo.process(); + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_writesysinfo.finished()) { return; } @@ -99,6 +120,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::WRITE_ENTIRE_MISSION: _writeentiremission.process(); + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_writeentiremission.finished()) { return; } @@ -107,6 +131,9 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::WRITE_ALL_RALLY_POINTS: _writeallrallypoints.process(); + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } if (!_writeallrallypoints.finished()) { return; }