From b681dc940af81558231cca70799040d861f5f4b3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 20 May 2021 12:34:13 +1000 Subject: [PATCH] AP_Logger: add support for AP_Logger into AP_Periph --- libraries/AP_Logger/AP_Logger.cpp | 6 ++++- libraries/AP_Logger/AP_Logger.h | 8 +++---- libraries/AP_Logger/AP_Logger_Backend.cpp | 2 ++ libraries/AP_Logger/AP_Logger_File.cpp | 4 ++-- .../AP_Logger_MAVLinkLogTransfer.cpp | 4 ++++ libraries/AP_Logger/LogFile.cpp | 2 ++ libraries/AP_Logger/LoggerMessageWriter.cpp | 20 +++++++++++++++++ libraries/AP_Logger/LoggerMessageWriter.h | 22 ++++++++++++++++--- 8 files changed, 58 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 53287de31a..0d1937fb62 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -92,12 +92,14 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = { // @User: Standard AP_GROUPINFO("_FILE_DSRMROT", 4, AP_Logger, _params.file_disarm_rot, 0), +#if HAL_LOGGING_MAVLINK_ENABLED // @Param: _MAV_BUFSIZE // @DisplayName: Maximum AP_Logger MAVLink Backend buffer size // @Description: Maximum amount of memory to allocate to AP_Logger-over-mavlink // @User: Advanced // @Units: kB AP_GROUPINFO("_MAV_BUFSIZE", 5, AP_Logger, _params.mav_bufsize, HAL_LOGGING_MAV_BUFSIZE), +#endif // @Param: _FILE_TIMEOUT // @DisplayName: Timeout before giving up on file writes @@ -136,7 +138,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types) _params.file_bufsize.convert_parameter_width(AP_PARAM_INT8); if (hal.util->was_watchdog_armed()) { - gcs().send_text(MAV_SEVERITY_INFO, "Forcing logging for watchdog reset"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing logging for watchdog reset"); _params.log_disarmed.set(1); } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -833,7 +835,9 @@ void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, const mavlink_message_t &m } void AP_Logger::periodic_tasks() { +#ifndef HAL_BUILD_AP_PERIPH handle_log_send(); +#endif FOR_EACH_BACKEND(periodic_tasks()); } diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 2731bb5892..65a25b8624 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -12,19 +12,19 @@ // set default for HAL_LOGGING_DATAFLASH_ENABLED #ifndef HAL_LOGGING_DATAFLASH_ENABLED #ifdef HAL_LOGGING_DATAFLASH - #define HAL_LOGGING_DATAFLASH_ENABLED 1 + #define HAL_LOGGING_DATAFLASH_ENABLED HAL_LOGGING_ENABLED #else #define HAL_LOGGING_DATAFLASH_ENABLED 0 #endif #endif #ifndef HAL_LOGGING_MAVLINK_ENABLED - #define HAL_LOGGING_MAVLINK_ENABLED 1 + #define HAL_LOGGING_MAVLINK_ENABLED HAL_LOGGING_ENABLED #endif #ifndef HAL_LOGGING_FILESYSTEM_ENABLED #if HAVE_FILESYSTEM_SUPPORT - #define HAL_LOGGING_FILESYSTEM_ENABLED 1 + #define HAL_LOGGING_FILESYSTEM_ENABLED HAL_LOGGING_ENABLED #else #define HAL_LOGGING_FILESYSTEM_ENABLED 0 #endif @@ -32,7 +32,7 @@ #ifndef HAL_LOGGING_SITL_ENABLED #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - #define HAL_LOGGING_SITL_ENABLED 1 + #define HAL_LOGGING_SITL_ENABLED HAL_LOGGING_ENABLED #else #define HAL_LOGGING_SITL_ENABLED 0 #endif diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 3e4f5f2274..e7936f4ccc 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -483,6 +483,7 @@ bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...) return Write_Message(msg); } +#if HAL_RALLY_ENABLED // Write rally points bool AP_Logger_Backend::Write_RallyPoint(uint8_t total, uint8_t sequence, @@ -506,6 +507,7 @@ bool AP_Logger_Backend::Write_Rally() // kick off asynchronous write: return _startup_messagewriter->writeallrallypoints(); } +#endif /* convert a list entry number back into a log number (which can then diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 7f99dc6e4a..ddc9ca73ea 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -148,7 +148,7 @@ void AP_Logger_File::periodic_1Hz() if (io_thread_warning_decimation_counter == 0 && _initialised) { // we don't print this error unless we did initialise. When _initialised is set to true // we register the IO timer callback - gcs().send_text(MAV_SEVERITY_CRITICAL, "AP_Logger: stuck thread (%s)", last_io_operation); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AP_Logger: stuck thread (%s)", last_io_operation); } if (io_thread_warning_decimation_counter++ > 57) { io_thread_warning_decimation_counter = 0; @@ -972,7 +972,7 @@ bool AP_Logger_File::io_thread_alive() const // if the io thread hasn't had a heartbeat in a while then it is // considered dead. Three seconds is enough time for a sdcard remount. uint32_t timeout_ms = 3000; -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) // the IO thread is working with hardware - writing to a physical // disk. Unfortunately these hardware devices do not obey our // SITL speedup options, so we allow for it here. diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index 8c7bfd9b62..ae529c5dca 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -22,6 +22,8 @@ #include #include // for LOG_ENTRY +#ifndef HAL_NO_GCS + extern const AP_HAL::HAL& hal; // We avoid doing log messages when timing is critical: @@ -328,3 +330,5 @@ bool AP_Logger::handle_log_send_data() } return true; } + +#endif diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 7d76ee255d..3607d6599f 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -264,11 +264,13 @@ bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission, return WriteBlock(&pkt, sizeof(pkt)); } +#if HAL_MISSION_ENABLED bool AP_Logger_Backend::Write_EntireMission() { // kick off asynchronous write: return _startup_messagewriter->writeentiremission(); } +#endif // Write a text message to the log bool AP_Logger_Backend::Write_Message(const char *message) diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index d15527b674..c5d236b01a 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -24,7 +24,11 @@ void LoggerMessageWriter::reset() bool LoggerMessageWriter::out_of_time_for_writing_messages() const { +#if HAL_SCHEDULER_ENABLED return AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US; +#else + return false; +#endif } void LoggerMessageWriter_DFLogStart::reset() @@ -34,8 +38,12 @@ void LoggerMessageWriter_DFLogStart::reset() _fmt_done = false; _params_done = false; _writesysinfo.reset(); +#if HAL_MISSION_ENABLED _writeentiremission.reset(); +#endif +#if HAL_RALLY_ENABLED _writeallrallypoints.reset(); +#endif stage = Stage::FORMATS; next_format_to_send = 0; @@ -49,7 +57,11 @@ bool LoggerMessageWriter_DFLogStart::out_of_time_for_writing_messages() const { if (stage == Stage::FORMATS) { // write out the FMT messages as fast as we can +#if HAL_SCHEDULER_ENABLED return AP::scheduler().time_available_usec() == 0; +#else + return false; +#endif } return LoggerMessageWriter::out_of_time_for_writing_messages(); } @@ -122,18 +134,22 @@ void LoggerMessageWriter_DFLogStart::process() return; } } +#if HAL_MISSION_ENABLED if (!_writeentiremission.finished()) { _writeentiremission.process(); if (!_writeentiremission.finished()) { return; } } +#endif +#if HAL_RALLY_ENABLED if (!_writeallrallypoints.finished()) { _writeallrallypoints.process(); if (!_writeallrallypoints.finished()) { return; } } +#endif stage = Stage::VEHICLE_MESSAGES; FALLTHROUGH; @@ -157,6 +173,7 @@ void LoggerMessageWriter_DFLogStart::process() _finished = true; } +#if HAL_MISSION_ENABLED bool LoggerMessageWriter_DFLogStart::writeentiremission() { if (stage != Stage::DONE) { @@ -167,7 +184,9 @@ bool LoggerMessageWriter_DFLogStart::writeentiremission() _writeentiremission.reset(); return true; } +#endif +#if HAL_RALLY_ENABLED bool LoggerMessageWriter_DFLogStart::writeallrallypoints() { if (stage != Stage::DONE) { @@ -178,6 +197,7 @@ bool LoggerMessageWriter_DFLogStart::writeallrallypoints() _writeallrallypoints.reset(); return true; } +#endif void LoggerMessageWriter_WriteSysInfo::reset() { diff --git a/libraries/AP_Logger/LoggerMessageWriter.h b/libraries/AP_Logger/LoggerMessageWriter.h index 0744d1f96e..b9a4d5b210 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.h +++ b/libraries/AP_Logger/LoggerMessageWriter.h @@ -75,17 +75,25 @@ private: class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter { public: LoggerMessageWriter_DFLogStart() : - _writesysinfo(), - _writeentiremission(), - _writeallrallypoints() + _writesysinfo() +#if HAL_MISSION_ENABLED + , _writeentiremission() +#endif +#if HAL_RALLY_ENABLED + , _writeallrallypoints() +#endif { } virtual void set_logger_backend(class AP_Logger_Backend *backend) override { LoggerMessageWriter::set_logger_backend(backend); _writesysinfo.set_logger_backend(backend); +#if HAL_MISSION_ENABLED _writeentiremission.set_logger_backend(backend); +#endif +#if HAL_RALLY_ENABLED _writeallrallypoints.set_logger_backend(backend); +#endif } bool out_of_time_for_writing_messages() const; @@ -97,8 +105,12 @@ public: // reset some writers so we push stuff out to logs again. Will // only work if we are in state DONE! +#if HAL_MISSION_ENABLED bool writeentiremission(); +#endif +#if HAL_RALLY_ENABLED bool writeallrallypoints(); +#endif private: @@ -130,6 +142,10 @@ private: LoggerMessageWriter_WriteSysInfo _writesysinfo; +#if HAL_MISSION_ENABLED LoggerMessageWriter_WriteEntireMission _writeentiremission; +#endif +#if HAL_RALLY_ENABLED LoggerMessageWriter_WriteAllRallyPoints _writeallrallypoints; +#endif };