From 1a2abae902b8c0b8bd456abc93b523cc22214101 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 19 Oct 2019 09:19:29 -0600 Subject: [PATCH] Plane: reduce QTUN log rate to 25Hz --- ArduPlane/quadplane.cpp | 13 +++++++++++-- ArduPlane/quadplane.h | 3 +++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1c327a5943..186fcf8b7c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1885,9 +1885,18 @@ void QuadPlane::motors_output(bool run_rate_controller) motors->output(); if (motors->armed() && motors->get_spool_state() != AP_Motors::SpoolState::SHUT_DOWN) { - plane.logger.Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control); - Log_Write_QControl_Tuning(); const uint32_t now = AP_HAL::millis(); + + // log RATE at main loop rate + plane.logger.Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control); + + // log QTUN at 25 Hz + if (now - last_qtun_log_ms > 40) { + last_qtun_log_ms = now; + Log_Write_QControl_Tuning(); + } + + // log CTRL at 10 Hz if (now - last_ctrl_log_ms > 100) { last_ctrl_log_ms = now; attitude_control->control_monitor_log(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 859bf495f6..337fa43f0d 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -414,6 +414,9 @@ private: // time of last control log message uint32_t last_ctrl_log_ms; + // time of last QTUN log message + uint32_t last_qtun_log_ms; + // types of tilt mechanisms enum {TILT_TYPE_CONTINUOUS =0, TILT_TYPE_BINARY =1,