From 2945de4796cc3079af04dba81e056e9f6bc3598f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 8 Nov 2020 14:32:24 +1100
Subject: [PATCH] AP_Logger: use WITH_SEMAPHORE in _WritePrioritisedBlock

this prevents a 1ms delay when waiting for the semaphore
---
 libraries/AP_Logger/AP_Logger_File.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp
index 7c6cc790e8..2e4f1784fc 100644
--- a/libraries/AP_Logger/AP_Logger_File.cpp
+++ b/libraries/AP_Logger/AP_Logger_File.cpp
@@ -490,6 +490,8 @@ bool AP_Logger_File::StartNewLogOK() const
 /* Write a block of data at current offset */
 bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
 {
+    WITH_SEMAPHORE(semaphore);
+
     if (! WriteBlockCheckStartupMessages()) {
         _dropped++;
         return false;
@@ -502,10 +504,7 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
     return true;
 #endif
 
-    if (!semaphore.take(1)) {
-        return false;
-    }
-        
+
     uint32_t space = _writebuf.space();
 
     if (_writing_startup_messages &&
@@ -519,7 +518,6 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
         if (!must_dribble &&
             space < non_messagewriter_message_reserved_space(_writebuf.get_size())) {
             // this message isn't dropped, it will be sent again...
-            semaphore.give();
             return false;
         }
         last_messagewrite_message_sent = now;
@@ -527,7 +525,6 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
         // we reserve some amount of space for critical messages:
         if (!is_critical && space < critical_message_reserved_space(_writebuf.get_size())) {
             _dropped++;
-            semaphore.give();
             return false;
         }
     }
@@ -536,13 +533,11 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
     if (space < size) {
         hal.util->perf_count(_perf_overruns);
         _dropped++;
-        semaphore.give();
         return false;
     }
 
     _writebuf.write((uint8_t*)pBuffer, size);
     df_stats_gather(size, _writebuf.space());
-    semaphore.give();
     return true;
 }