diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 947975a0a1686..ad387e571c473 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -317,6 +317,27 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @User: Advanced AP_GROUPINFO("THR_G_BOOST", 7, AC_AttitudeControl_Multi, _throttle_gain_boost, 0.0f), + // @Param: PD_RLL_SCALE + // @DisplayName: PD scale factor for Roll + // @Description: Scale factor that is to be applied to Roll PDs + // @Range: 0.1 1.0 + // @User: Advanced + AP_GROUPINFO("PD_RLL_SCALE", 8, AC_AttitudeControl_Multi, _pd_scale_roll, 1.0f), + + // @Param: PD_PIT_SCALE + // @DisplayName: PD scale factor for Pitch + // @Description: Scale factor that is to be applied to Pitch PDs + // @Range: 0.1 1.0 + // @User: Advanced + AP_GROUPINFO("PD_PIT_SCALE", 9, AC_AttitudeControl_Multi, _pd_scale_pitch, 1.0f), + + // @Param: PD_YAW_SCALE + // @DisplayName: PD scale factor for Yaw + // @Description: Scale factor that is to be applied to Yaw PDs + // @Range: 0.1 1.0 + // @User: Advanced + AP_GROUPINFO("PD_YAW_SCALE", 10, AC_AttitudeControl_Multi, _pd_scale_yaw, 1.0f), + AP_GROUPEND }; @@ -464,7 +485,10 @@ void AC_AttitudeControl_Multi::rate_controller_run() _actuator_sysid.zero(); _pd_scale_used = _pd_scale; - _pd_scale = VECTORF_111; + // reset the scale back to the default + _pd_scale.x = _pd_scale_roll; + _pd_scale.y = _pd_scale_pitch; + _pd_scale.z = _pd_scale_yaw; control_monitor_update(); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 55359a1edb6fb..2cd622ffcde8a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -149,4 +149,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { // angle_p/pd boost multiplier AP_Float _throttle_gain_boost; + AP_Float _pd_scale_roll; // PD roll default scale factor + AP_Float _pd_scale_pitch; // PD pitch default scale factor + AP_Float _pd_scale_yaw; // PD yaw default scale factor }; diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 166be7b7b3122..e0447668dff00 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -190,7 +190,7 @@ void AC_PID::set_notch_sample_rate(float sample_rate) // target and error are filtered // the derivative is then calculated and filtered // the integral is then updated based on the setting of the limit flag -float AC_PID::update_all(float target, float measurement, float dt, bool limit, float boost) +float AC_PID::update_all(float target, float measurement, float dt, bool limit, float scale) { // don't process inf or NaN if (!isfinite(target) || !isfinite(measurement)) { @@ -252,9 +252,9 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit, P_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod; - // boost output if required - P_out *= boost; - D_out *= boost; + // scale output if required + P_out *= scale; + D_out *= scale; _pid_info.PD_limit = false; // Apply PD sum limit if enabled @@ -318,6 +318,7 @@ void AC_PID::update_i(float dt, bool limit) } else { _integrator = 0.0f; } + _pid_info.I = _integrator; _pid_info.limit = limit; diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index fbbebcac7db78..a5b476640233e 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -62,7 +62,7 @@ class AC_PID { // target and error are filtered // the derivative is then calculated and filtered // the integral is then updated based on the setting of the limit flag - float update_all(float target, float measurement, float dt, bool limit = false, float boost = 1.0f); + float update_all(float target, float measurement, float dt, bool limit = false, float scale = 1.0f); // update_error - set error input to PID controller and calculate outputs // target is set to zero and error is set and filtered