diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 73336fe3a4..546d941d1e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -561,7 +561,7 @@ void AC_AutoTune::control_attitude() case TWITCHING: { // Run the twitching step - // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) + load_gains(GAIN_TWITCH); // disable rate limits attitude_control->use_sqrt_controller(false); @@ -664,7 +664,7 @@ void AC_AutoTune::control_attitude() // log this iterations lean angle and rotation rate Log_Write_AutoTuneDetails(lean_angle, rotation_rate); DataFlash_Class::instance()->Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control); - + log_pids(); break; } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index ef043d5c3a..106fe21838 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -53,6 +53,9 @@ protected: // init pos controller Z velocity and accel limits virtual void init_z_limits() = 0; + // log PIDs at full rate for during twitch + virtual void log_pids() = 0; + // start tune - virtual so that vehicle code can add additional pre-conditions virtual bool start(void);