diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index c8008b61d4..0cc30e59cb 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -424,9 +424,17 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) } // setup filters to be suitable for time constant and gyro filter - rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI)); + // filtering T can prevent P/D oscillation being seen, so allow the + // user to switch it off + if (!has_option(DISABLE_FLTT_UPDATE)) { + rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI)); + } rpid.filt_E_hz().set(0); - rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5); + // filtering D at the same level as VTOL can allow unwanted oscillations to be seen, + // so allow the user to switch it off and select their own (usually lower) value + if (!has_option(DISABLE_FLTD_UPDATE)) { + rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5); + } current.FF = FF; current.P = P; diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 1859a2489f..67b6cf07f9 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -25,6 +25,11 @@ public: AUTOTUNE_YAW = 2, }; + enum Options { + DISABLE_FLTD_UPDATE = 0, + DISABLE_FLTT_UPDATE = 1 + }; + struct PACKED log_ATRP { LOG_PACKET_HEADER; uint64_t time_us; @@ -116,6 +121,10 @@ private: // update rmax and tau towards target void update_rmax(); + bool has_option(Options option) { + return (aparm.autotune_options.get() & uint32_t(1<