diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index 1c00a085b4..abb895f280 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -554,11 +554,15 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, // 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()) { + const uint32_t now = AP_HAL::millis(); + const bool must_dribble = (now - last_messagewrite_message_sent) > 100; + if (!must_dribble && + space < non_messagewriter_message_reserved_space()) { // this message isn't dropped, it will be sent again... semaphore->give(); return false; } + last_messagewrite_message_sent = now; } else { // we reserve some amount of space for critical messages: if (!is_critical && space < critical_message_reserved_space()) { diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index 93b1f86924..7a0e2a6ec0 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -136,6 +136,7 @@ private: } return ret; }; + uint32_t last_messagewrite_message_sent; // free-space checks; filling up SD cards under NuttX leads to // corrupt filesystems which cause loss of data, failure to gather