From 63320f140a57b1263152aee1c076757af24dee39 Mon Sep 17 00:00:00 2001
From: chobits <chobits@itri.org.tw>
Date: Thu, 9 Apr 2020 15:50:54 +0800
Subject: [PATCH] AP_Logger: constraints time spend in header writing, more
 complete

---
 libraries/AP_Logger/LoggerMessageWriter.cpp | 29 ++++++++++++++++++---
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp
index 750579dab8..49c18ba761 100644
--- a/libraries/AP_Logger/LoggerMessageWriter.cpp
+++ b/libraries/AP_Logger/LoggerMessageWriter.cpp
@@ -6,6 +6,8 @@
 #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
@@ -36,13 +38,13 @@ void LoggerMessageWriter_DFLogStart::reset()
 
 void LoggerMessageWriter_DFLogStart::process()
 {
-    if (AP::scheduler().time_available_usec() < 200) {
-        return;
-    }
     switch(stage) {
     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!
             }
@@ -54,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!
             }
@@ -64,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!
             }
@@ -74,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!
             }
@@ -84,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;
             }
@@ -95,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;
         }
@@ -103,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;
         }
@@ -111,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;
         }