diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 13ac1f697b..89e1aa5536 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -557,6 +557,7 @@ private: // last time we handled a log-transfer-over-mavlink message: uint32_t _last_mavlink_log_transfer_message_handled_ms; + bool _warned_log_disarm; // true if we have sent a message warning to disarm for logging // next log list entry to send uint16_t _log_next_list_entry; @@ -592,7 +593,6 @@ private: // can be used by other subsystems to detect if they should log data uint8_t _log_start_count; - bool should_handle_log_message() const; void handle_log_message(class GCS_MAVLINK &, const mavlink_message_t &msg); void handle_log_request_list(class GCS_MAVLINK &, const mavlink_message_t &msg); diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index ae529c5dca..28122180df 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -26,27 +26,23 @@ extern const AP_HAL::HAL& hal; -// We avoid doing log messages when timing is critical: -bool AP_Logger::should_handle_log_message() const -{ - if (!WritesEnabled()) { - // this is currently used as a proxy for "in_mavlink_delay" - return false; - } - if (vehicle_is_armed()) { - return false; - } - return true; -} - /** handle all types of log download requests from the GCS */ void AP_Logger::handle_log_message(GCS_MAVLINK &link, const mavlink_message_t &msg) { - if (!should_handle_log_message()) { + if (!WritesEnabled()) { + // this is currently used as a proxy for "in_mavlink_delay" return; } + if (vehicle_is_armed()) { + if (!_warned_log_disarm) { + link.send_text(MAV_SEVERITY_ERROR, "Disarm for log download"); + _warned_log_disarm = true; + } + return; + } + _warned_log_disarm = false; _last_mavlink_log_transfer_message_handled_ms = AP_HAL::millis(); switch (msg.msgid) { case MAVLINK_MSG_ID_LOG_REQUEST_LIST: