Browse Source

Plane: reduce QTUN log rate to 25Hz

master
Mark Whitehorn 5 years ago committed by Andrew Tridgell
parent
commit
1a2abae902
  1. 11
      ArduPlane/quadplane.cpp
  2. 3
      ArduPlane/quadplane.h

11
ArduPlane/quadplane.cpp

@ -1885,9 +1885,18 @@ void QuadPlane::motors_output(bool run_rate_controller) @@ -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) {
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();
const uint32_t now = AP_HAL::millis();
}
// log CTRL at 10 Hz
if (now - last_ctrl_log_ms > 100) {
last_ctrl_log_ms = now;
attitude_control->control_monitor_log();

3
ArduPlane/quadplane.h

@ -414,6 +414,9 @@ private: @@ -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,

Loading…
Cancel
Save