diff --git a/libraries/AC_AttitudeControl/ControlMonitor.cpp b/libraries/AC_AttitudeControl/ControlMonitor.cpp index 59ef9734ea4cb..37fadc2600cbf 100644 --- a/libraries/AC_AttitudeControl/ControlMonitor.cpp +++ b/libraries/AC_AttitudeControl/ControlMonitor.cpp @@ -29,7 +29,7 @@ void AC_AttitudeControl::control_monitor_update(void) control_monitor_filter_pid(iroll.D, _control_monitor.rms_roll_D); const AP_Logger::PID_Info &ipitch = get_rate_pitch_pid().get_pid_info(); - control_monitor_filter_pid(ipitch.P + iroll.FF, _control_monitor.rms_pitch_P); + control_monitor_filter_pid(ipitch.P + ipitch.FF, _control_monitor.rms_pitch_P); control_monitor_filter_pid(ipitch.D, _control_monitor.rms_pitch_D); const AP_Logger::PID_Info &iyaw = get_rate_yaw_pid().get_pid_info(); diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index 4fec4b55f8b4a..638c80b3af894 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -49,7 +49,7 @@ const AP_Param::GroupInfo AP_Tuning::var_info[] = { // @Param: ERR_THRESH // @DisplayName: Controller error threshold - // @Description: This sets the controller error threshold above which an alarm will sound and a message will be sent to the GCS to warn of controller instability + // @Description: This sets the controller error threshold above which an alarm will sound and a message will be sent to the GCS to warn of controller instability while tuning. The error is the rms value of the P+D corrections in the loop. High values in hover indicate possible instability due to too high PID gains or excessively high D to P gain ratios.-1 will disable this message. // @Range: 0 1 // @User: Standard AP_GROUPINFO("ERR_THRESH", 7, AP_Tuning, error_threshold, 0.15f), @@ -342,7 +342,7 @@ const char *AP_Tuning::get_tuning_name(uint8_t parm) void AP_Tuning::check_controller_error(void) { float err = controller_error(current_parm); - if (err > error_threshold) { + if (err > error_threshold && !mid_point_wait && error_threshold > 0) { uint32_t now = AP_HAL::millis(); if (now - last_controller_error_ms > 2000 && hal.util->get_soft_armed()) { AP_Notify::events.tune_error = 1;