From b897551e0b00ad064350ed6acd65f4a4e33444a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Dec 2018 13:26:45 +1100 Subject: [PATCH] Copter: log PIDs at full rate during AUTOTUNE twitch --- ArduCopter/mode.h | 1 + ArduCopter/mode_autotune.cpp | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 39289674a0..9ade4f158c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -455,6 +455,7 @@ protected: void get_pilot_desired_rp_yrate_cd(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_rate_cds) override; void init_z_limits() override; void Log_Write_Event(enum at_event id) override; + void log_pids() override; }; class ModeAutoTune : public Mode { diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 251a5722fd..2953fd5559 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -112,6 +112,13 @@ void Copter::AutoTune::init_z_limits() copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); } +void Copter::AutoTune::log_pids() +{ + copter.DataFlash.Log_Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info()); + copter.DataFlash.Log_Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info()); + copter.DataFlash.Log_Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info()); +} + /* Write an event packet. This maps from AC_AutoTune event IDs to