diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 63607624b0..187e825540 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -440,9 +440,8 @@ bool AP_Logger_Backend::Write_RallyPoint(uint8_t total, } // Write rally points -void AP_Logger_Backend::Write_Rally() +bool AP_Logger_Backend::Write_Rally() { - LoggerMessageWriter_WriteAllRallyPoints writer; - writer.set_logger_backend(this); - writer.process(); + // kick off asynchronous write: + return _startup_messagewriter->writeallrallypoints(); } diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index dc3719e5aa..10aa4bbe01 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -83,11 +83,11 @@ public: uint8_t num_multipliers() const; const struct MultiplierStructure *multiplier(uint8_t multiplier) const; - void Write_EntireMission(); + bool Write_EntireMission(); bool Write_RallyPoint(uint8_t total, uint8_t sequence, const RallyLocation &rally_point); - void Write_Rally(); + bool Write_Rally(); bool Write_Format(const struct LogStructure *structure); bool Write_Message(const char *message); bool Write_MessageF(const char *fmt, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 20f8007ad3..aa5e9b65cc 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -448,11 +448,10 @@ bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission, return WriteBlock(&pkt, sizeof(pkt)); } -void AP_Logger_Backend::Write_EntireMission() +bool AP_Logger_Backend::Write_EntireMission() { - LoggerMessageWriter_WriteEntireMission writer{}; - writer.set_logger_backend(this); - writer.process(); + // kick off asynchronous write: + return _startup_messagewriter->writeentiremission(); } // Write a text message to the log diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index 49c18ba761..ee3b7fd3b5 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -104,38 +104,27 @@ void LoggerMessageWriter_DFLogStart::process() ap = AP_Param::next_scalar(&token, &type); } - stage = Stage::SYSINFO; + stage = Stage::RUNNING_SUBWRITERS; FALLTHROUGH; - case Stage::SYSINFO: - _writesysinfo.process(); - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; - } + case Stage::RUNNING_SUBWRITERS: if (!_writesysinfo.finished()) { - return; - } - stage = Stage::WRITE_ENTIRE_MISSION; - FALLTHROUGH; - - case Stage::WRITE_ENTIRE_MISSION: - _writeentiremission.process(); - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; + _writesysinfo.process(); + if (!_writesysinfo.finished()) { + return; + } } if (!_writeentiremission.finished()) { - return; - } - stage = Stage::WRITE_ALL_RALLY_POINTS; - FALLTHROUGH; - - case Stage::WRITE_ALL_RALLY_POINTS: - _writeallrallypoints.process(); - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; + _writeentiremission.process(); + if (!_writeentiremission.finished()) { + return; + } } if (!_writeallrallypoints.finished()) { - return; + _writeallrallypoints.process(); + if (!_writeallrallypoints.finished()) { + return; + } } stage = Stage::VEHICLE_MESSAGES; FALLTHROUGH; @@ -160,6 +149,28 @@ void LoggerMessageWriter_DFLogStart::process() _finished = true; } +bool LoggerMessageWriter_DFLogStart::writeentiremission() +{ + if (stage != Stage::DONE) { + return false; + } + stage = Stage::RUNNING_SUBWRITERS; + _finished = false; + _writeentiremission.reset(); + return true; +} + +bool LoggerMessageWriter_DFLogStart::writeallrallypoints() +{ + if (stage != Stage::DONE) { + return false; + } + stage = Stage::RUNNING_SUBWRITERS; + _finished = false; + _writeallrallypoints.reset(); + return true; +} + void LoggerMessageWriter_WriteSysInfo::reset() { LoggerMessageWriter::reset(); @@ -250,6 +261,9 @@ void LoggerMessageWriter_WriteAllRallyPoints::process() case Stage::WRITE_ALL_RALLY_POINTS: while (_rally_number_to_send < _rally->get_rally_total()) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } RallyLocation rallypoint; if (_rally->get_rally_point_with_index(_rally_number_to_send, rallypoint)) { if (!_logger_backend->Write_RallyPoint( @@ -297,6 +311,9 @@ void LoggerMessageWriter_WriteEntireMission::process() { case Stage::WRITE_MISSION_ITEMS: { AP_Mission::Mission_Command cmd; while (_mission_number_to_send < _mission->num_commands()) { + if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + return; + } // upon failure to write the mission we will re-read from // storage; this could be improved. if (_mission->read_cmd_from_storage(_mission_number_to_send,cmd)) { diff --git a/libraries/AP_Logger/LoggerMessageWriter.h b/libraries/AP_Logger/LoggerMessageWriter.h index 120bdde2ad..799cb9dcfa 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.h +++ b/libraries/AP_Logger/LoggerMessageWriter.h @@ -91,6 +91,11 @@ public: void process() override; bool fmt_done() { return _fmt_done; } + // reset some writers so we push stuff out to logs again. Will + // only work if we are in state DONE! + bool writeentiremission(); + bool writeallrallypoints(); + private: enum Stage { @@ -99,10 +104,8 @@ private: MULTIPLIERS, FORMAT_UNITS, PARMS, - SYSINFO, - WRITE_ENTIRE_MISSION, - WRITE_ALL_RALLY_POINTS, VEHICLE_MESSAGES, + RUNNING_SUBWRITERS, // must be last thing to run as we can redo bits of these DONE, };