mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
5328e70b50
commit
313e8e39b6
|
@ -163,6 +163,15 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = {
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("_BLK_RATEMAX", 10, AP_Logger, _params.blk_ratemax, 0),
|
AP_GROUPINFO("_BLK_RATEMAX", 10, AP_Logger, _params.blk_ratemax, 0),
|
||||||
#endif
|
#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
|
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 now = AP_HAL::millis();
|
||||||
uint32_t persist_ms = HAL_LOGGER_ARM_PERSIST*1000U;
|
uint32_t persist_ms = HAL_LOGGER_ARM_PERSIST*1000U;
|
||||||
if (_force_long_log_persist) {
|
if (_force_long_log_persist) {
|
||||||
|
@ -1495,6 +1496,24 @@ bool AP_Logger::log_while_disarmed(void) const
|
||||||
return false;
|
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
|
#if HAL_LOGGER_FILE_CONTENTS_ENABLED
|
||||||
void AP_Logger::prepare_at_arming_sys_file_logging()
|
void AP_Logger::prepare_at_arming_sys_file_logging()
|
||||||
{
|
{
|
||||||
|
|
|
@ -311,6 +311,7 @@ public:
|
||||||
void set_force_log_disarmed(bool force_logging) { _force_log_disarmed = force_logging; }
|
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; }
|
void set_long_log_persist(bool b) { _force_long_log_persist = b; }
|
||||||
bool log_while_disarmed(void) const;
|
bool log_while_disarmed(void) const;
|
||||||
|
bool in_log_persistance(void) const;
|
||||||
uint8_t log_replay(void) const { return _params.log_replay; }
|
uint8_t log_replay(void) const { return _params.log_replay; }
|
||||||
|
|
||||||
vehicle_startup_message_Writer _vehicle_messages;
|
vehicle_startup_message_Writer _vehicle_messages;
|
||||||
|
@ -336,6 +337,7 @@ public:
|
||||||
AP_Float file_ratemax;
|
AP_Float file_ratemax;
|
||||||
AP_Float mav_ratemax;
|
AP_Float mav_ratemax;
|
||||||
AP_Float blk_ratemax;
|
AP_Float blk_ratemax;
|
||||||
|
AP_Float disarm_ratemax;
|
||||||
} _params;
|
} _params;
|
||||||
|
|
||||||
const struct LogStructure *structure(uint16_t num) const;
|
const struct LogStructure *structure(uint16_t num) const;
|
||||||
|
|
|
@ -671,22 +671,24 @@ void AP_Logger_Backend::df_stats_log() {
|
||||||
|
|
||||||
|
|
||||||
// class to handle rate limiting of log messages
|
// class to handle rate limiting of log messages
|
||||||
AP_Logger_RateLimiter::AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_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)
|
: 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
|
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) {
|
if (front._log_pause) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
const uint16_t now = AP_HAL::millis16();
|
const uint16_t now = AP_HAL::millis16();
|
||||||
uint16_t delta_ms = now - last_send_ms[msgid];
|
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
|
// too soon
|
||||||
return false;
|
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)
|
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)
|
// no rate limiting if not paused and rate is zero(user changed the parameter)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -728,7 +736,7 @@ bool AP_Logger_RateLimiter::should_log(uint8_t msgid, bool writev_streaming)
|
||||||
last_sched_count[msgid] = sched_ticks;
|
last_sched_count[msgid] = sched_ticks;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
bool ret = should_log_streaming(msgid);
|
bool ret = should_log_streaming(msgid, rate_hz);
|
||||||
if (ret) {
|
if (ret) {
|
||||||
last_return.set(msgid);
|
last_return.set(msgid);
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -12,15 +12,16 @@ class LoggerMessageWriter_DFLogStart;
|
||||||
class AP_Logger_RateLimiter
|
class AP_Logger_RateLimiter
|
||||||
{
|
{
|
||||||
public:
|
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
|
// return true if message passes the rate limit test
|
||||||
bool should_log(uint8_t msgid, bool writev_streaming);
|
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:
|
private:
|
||||||
const AP_Logger &front;
|
const AP_Logger &front;
|
||||||
const AP_Float &rate_limit_hz;
|
const AP_Float &rate_limit_hz;
|
||||||
|
const AP_Float &disarm_rate_limit_hz;
|
||||||
|
|
||||||
// time in ms we last sent this message
|
// time in ms we last sent this message
|
||||||
uint16_t last_send_ms[256];
|
uint16_t last_send_ms[256];
|
||||||
|
|
|
@ -307,9 +307,12 @@ void AP_Logger_Block::periodic_1Hz()
|
||||||
{
|
{
|
||||||
AP_Logger_Backend::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
|
// 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()) {
|
if (!io_thread_alive()) {
|
||||||
|
|
|
@ -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
|
// 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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -530,9 +530,12 @@ void AP_Logger_MAVLink::periodic_10Hz(const uint32_t now)
|
||||||
}
|
}
|
||||||
void AP_Logger_MAVLink::periodic_1Hz()
|
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
|
// 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 &&
|
if (_sending_to_client &&
|
||||||
|
|
Loading…
Reference in New Issue