From 313e8e39b6e62b93d56465c23df971a9f0e8543f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 11 Apr 2023 20:16:41 +1000 Subject: [PATCH] AP_Logger: added LOG_DARM_RATEMAX this sets the logging rate max when disarmed. In combination with LOG_DISARMED=3 it gives a very nice setup to get always on logging with very little addition to the log sizes. It is particularly useful in combination with LOG_REPLAY=1 --- libraries/AP_Logger/AP_Logger.cpp | 41 +++++++++++++++++------ libraries/AP_Logger/AP_Logger.h | 2 ++ libraries/AP_Logger/AP_Logger_Backend.cpp | 20 +++++++---- libraries/AP_Logger/AP_Logger_Backend.h | 5 +-- libraries/AP_Logger/AP_Logger_Block.cpp | 7 ++-- libraries/AP_Logger/AP_Logger_File.cpp | 7 ++-- libraries/AP_Logger/AP_Logger_MAVLink.cpp | 7 ++-- 7 files changed, 64 insertions(+), 25 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index c9f90ed4a5..06c27e60c7 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -163,6 +163,15 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = { // @User: Standard AP_GROUPINFO("_BLK_RATEMAX", 10, AP_Logger, _params.blk_ratemax, 0), #endif + + // @Param: _DARM_RATEMAX + // @DisplayName: Maximum logging rate when disarmed + // @Description: This sets the maximum rate that streaming log messages will be logged to any backend when disarmed. A value of zero means that the normal backend rate limit is applied. + // @Units: Hz + // @Range: 0 1000 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("_DARM_RATEMAX", 11, AP_Logger, _params.disarm_ratemax, 0), AP_GROUPEND }; @@ -1461,19 +1470,11 @@ void AP_Logger::Write_Error(LogErrorSubsystem sub_system, } /* - return true if we should log while disarmed + return true if we are in a logging persistance state, where we keep + logging after a disarm or an arming failure */ -bool AP_Logger::log_while_disarmed(void) const +bool AP_Logger::in_log_persistance(void) const { - if (_force_log_disarmed) { - return true; - } - if (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED || - _params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_DISCARD || - (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_NOT_USB && !hal.gpio->usb_connected())) { - return true; - } - uint32_t now = AP_HAL::millis(); uint32_t persist_ms = HAL_LOGGER_ARM_PERSIST*1000U; if (_force_long_log_persist) { @@ -1495,6 +1496,24 @@ bool AP_Logger::log_while_disarmed(void) const return false; } + +/* + return true if we should log while disarmed + */ +bool AP_Logger::log_while_disarmed(void) const +{ + if (_force_log_disarmed) { + return true; + } + if (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED || + _params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_DISCARD || + (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_NOT_USB && !hal.gpio->usb_connected())) { + return true; + } + + return in_log_persistance(); +} + #if HAL_LOGGER_FILE_CONTENTS_ENABLED void AP_Logger::prepare_at_arming_sys_file_logging() { diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 21715f7cc2..717ee5f604 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -311,6 +311,7 @@ public: void set_force_log_disarmed(bool force_logging) { _force_log_disarmed = force_logging; } void set_long_log_persist(bool b) { _force_long_log_persist = b; } bool log_while_disarmed(void) const; + bool in_log_persistance(void) const; uint8_t log_replay(void) const { return _params.log_replay; } vehicle_startup_message_Writer _vehicle_messages; @@ -336,6 +337,7 @@ public: AP_Float file_ratemax; AP_Float mav_ratemax; AP_Float blk_ratemax; + AP_Float disarm_ratemax; } _params; const struct LogStructure *structure(uint16_t num) const; diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 16f162ed7c..c9ec3e6f6b 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -671,22 +671,24 @@ void AP_Logger_Backend::df_stats_log() { // class to handle rate limiting of log messages -AP_Logger_RateLimiter::AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_limit_hz) - : front(_front), rate_limit_hz(_limit_hz) +AP_Logger_RateLimiter::AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_limit_hz, const AP_Float &_disarm_limit_hz) + : front(_front), + rate_limit_hz(_limit_hz), + disarm_rate_limit_hz(_disarm_limit_hz) { } /* return false if a streaming message should not be sent yet */ -bool AP_Logger_RateLimiter::should_log_streaming(uint8_t msgid) +bool AP_Logger_RateLimiter::should_log_streaming(uint8_t msgid, float rate_hz) { if (front._log_pause) { return false; } const uint16_t now = AP_HAL::millis16(); uint16_t delta_ms = now - last_send_ms[msgid]; - if (delta_ms < 1000.0 / rate_limit_hz.get()) { + if (is_positive(rate_hz) && delta_ms < 1000.0 / rate_hz) { // too soon return false; } @@ -700,7 +702,13 @@ bool AP_Logger_RateLimiter::should_log_streaming(uint8_t msgid) */ bool AP_Logger_RateLimiter::should_log(uint8_t msgid, bool writev_streaming) { - if (rate_limit_hz <= 0 && !front._log_pause) { + float rate_hz = rate_limit_hz; + if (!hal.util->get_soft_armed() && + !AP::logger().in_log_persistance() && + !is_zero(disarm_rate_limit_hz)) { + rate_hz = disarm_rate_limit_hz; + } + if (!is_positive(rate_hz) && !front._log_pause) { // no rate limiting if not paused and rate is zero(user changed the parameter) return true; } @@ -728,7 +736,7 @@ bool AP_Logger_RateLimiter::should_log(uint8_t msgid, bool writev_streaming) last_sched_count[msgid] = sched_ticks; #endif - bool ret = should_log_streaming(msgid); + bool ret = should_log_streaming(msgid, rate_hz); if (ret) { last_return.set(msgid); } else { diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index 650ffe4efc..5e5113a381 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -12,15 +12,16 @@ class LoggerMessageWriter_DFLogStart; class AP_Logger_RateLimiter { public: - AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_limit_hz); + AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_limit_hz, const AP_Float &_disarm_limit_hz); // return true if message passes the rate limit test bool should_log(uint8_t msgid, bool writev_streaming); - bool should_log_streaming(uint8_t msgid); + bool should_log_streaming(uint8_t msgid, float rate_hz); private: const AP_Logger &front; const AP_Float &rate_limit_hz; + const AP_Float &disarm_rate_limit_hz; // time in ms we last sent this message uint16_t last_send_ms[256]; diff --git a/libraries/AP_Logger/AP_Logger_Block.cpp b/libraries/AP_Logger/AP_Logger_Block.cpp index e261fe6df5..ba51432f14 100644 --- a/libraries/AP_Logger/AP_Logger_Block.cpp +++ b/libraries/AP_Logger/AP_Logger_Block.cpp @@ -307,9 +307,12 @@ void AP_Logger_Block::periodic_1Hz() { AP_Logger_Backend::periodic_1Hz(); - if (rate_limiter == nullptr && (_front._params.blk_ratemax > 0 || _front._log_pause)) { + if (rate_limiter == nullptr && + (_front._params.blk_ratemax > 0 || + _front._params.disarm_ratemax > 0 || + _front._log_pause)) { // setup rate limiting if log rate max > 0Hz or log pause of streaming entries is requested - rate_limiter = new AP_Logger_RateLimiter(_front, _front._params.blk_ratemax); + rate_limiter = new AP_Logger_RateLimiter(_front, _front._params.blk_ratemax, _front._params.disarm_ratemax); } if (!io_thread_alive()) { diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 351cf2f6bf..77324c5b31 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -164,9 +164,12 @@ void AP_Logger_File::periodic_1Hz() } } - if (rate_limiter == nullptr && (_front._params.file_ratemax > 0 || _front._log_pause)) { + if (rate_limiter == nullptr && + (_front._params.file_ratemax > 0 || + _front._params.disarm_ratemax > 0 || + _front._log_pause)) { // setup rate limiting if log rate max > 0Hz or log pause of streaming entries is requested - rate_limiter = new AP_Logger_RateLimiter(_front, _front._params.file_ratemax); + rate_limiter = new AP_Logger_RateLimiter(_front, _front._params.file_ratemax, _front._params.disarm_ratemax); } } diff --git a/libraries/AP_Logger/AP_Logger_MAVLink.cpp b/libraries/AP_Logger/AP_Logger_MAVLink.cpp index 69eb068c52..7184d84803 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLink.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLink.cpp @@ -530,9 +530,12 @@ void AP_Logger_MAVLink::periodic_10Hz(const uint32_t now) } void AP_Logger_MAVLink::periodic_1Hz() { - if (rate_limiter == nullptr && (_front._params.mav_ratemax > 0 || _front._log_pause)) { + if (rate_limiter == nullptr && + (_front._params.mav_ratemax > 0 || + _front._params.disarm_ratemax > 0 || + _front._log_pause)) { // setup rate limiting if log rate max > 0Hz or log pause of streaming entries is requested - rate_limiter = new AP_Logger_RateLimiter(_front, _front._params.mav_ratemax); + rate_limiter = new AP_Logger_RateLimiter(_front, _front._params.mav_ratemax, _front._params.disarm_ratemax); } if (_sending_to_client &&