diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index 396e264b2a..a2de4a51b4 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -95,11 +95,13 @@ 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 (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send)) || - check_process_limit(start_us)) { + if (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send))) { return; // call me again! } next_format_to_send++; + if (check_process_limit(start_us)) { + return; // call me again! + } } _fmt_done = true; stage = Stage::PARMS; @@ -107,12 +109,14 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::PARMS: { while (ap) { - if (!_logger_backend->Write_Parameter(ap, token, type, param_default) || - check_process_limit(start_us)) { + if (!_logger_backend->Write_Parameter(ap, token, type, param_default)) { return; } param_default = AP::logger().quiet_nanf(); ap = AP_Param::next_scalar(&token, &type, ¶m_default); + if (check_process_limit(start_us)) { + return; // call me again! + } } _params_done = true; @@ -122,33 +126,39 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::UNITS: while (_next_unit_to_send < _logger_backend->num_units()) { - if (!_logger_backend->Write_Unit(_logger_backend->unit(_next_unit_to_send)) || - check_process_limit(start_us)) { + if (!_logger_backend->Write_Unit(_logger_backend->unit(_next_unit_to_send))) { return; // call me again! } _next_unit_to_send++; + if (check_process_limit(start_us)) { + return; // call me again! + } } stage = Stage::MULTIPLIERS; FALLTHROUGH; case Stage::MULTIPLIERS: while (_next_multiplier_to_send < _logger_backend->num_multipliers()) { - if (!_logger_backend->Write_Multiplier(_logger_backend->multiplier(_next_multiplier_to_send)) || - check_process_limit(start_us)) { + if (!_logger_backend->Write_Multiplier(_logger_backend->multiplier(_next_multiplier_to_send))) { return; // call me again! } _next_multiplier_to_send++; + if (check_process_limit(start_us)) { + return; // call me again! + } } stage = Stage::UNITS; FALLTHROUGH; case Stage::FORMAT_UNITS: while (_next_format_unit_to_send < _logger_backend->num_types()) { - if (!_logger_backend->Write_Format_Units(_logger_backend->structure(_next_format_unit_to_send)) || - check_process_limit(start_us)) { + if (!_logger_backend->Write_Format_Units(_logger_backend->structure(_next_format_unit_to_send))) { return; // call me again! } _next_format_unit_to_send++; + if (check_process_limit(start_us)) { + return; // call me again! + } } stage = Stage::RUNNING_SUBWRITERS; FALLTHROUGH;