diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index e83abb39aeb87..56b9cca37a8b3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -336,10 +336,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = { AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors) : AC_AttitudeControl(ahrs, aparm, motors), - _motors_multi(motors), - _pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ), - _pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ), - _pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ) + _motors_multi(motors) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h index fb69f7094f0d6..c5a257016da50 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h @@ -83,9 +83,39 @@ class AC_AttitudeControl_Sub : public AC_AttitudeControl { float get_throttle_avg_max(float throttle_in); AP_MotorsMulticopter& _motors_multi; - AC_PID _pid_rate_roll; - AC_PID _pid_rate_pitch; - AC_PID _pid_rate_yaw; + + // Roll and Pitch rate PIDs share the same defaults: + const AC_PID::Defaults rp_defaults { + AC_PID::Defaults{ + .p = AC_ATC_SUB_RATE_RP_P, + .i = AC_ATC_SUB_RATE_RP_I, + .d = AC_ATC_SUB_RATE_RP_D, + .ff = 0.0f, + .imax = AC_ATC_SUB_RATE_RP_IMAX, + .filt_T_hz = AC_ATC_SUB_RATE_RP_FILT_HZ, + .filt_E_hz = 0.0, + .filt_D_hz = AC_ATC_SUB_RATE_RP_FILT_HZ, + .srmax = 0, + .srtau = 1.0 + } + }; + AC_PID _pid_rate_roll { rp_defaults }; + AC_PID _pid_rate_pitch { rp_defaults }; + + AC_PID _pid_rate_yaw { + AC_PID::Defaults{ + .p = AC_ATC_SUB_RATE_YAW_P, + .i = AC_ATC_SUB_RATE_YAW_I, + .d = AC_ATC_SUB_RATE_YAW_D, + .ff = 0.0f, + .imax = AC_ATC_SUB_RATE_YAW_IMAX, + .filt_T_hz = AC_ATC_SUB_RATE_YAW_FILT_HZ, + .filt_E_hz = 0.0f, + .filt_D_hz = AC_ATC_SUB_RATE_YAW_FILT_HZ, + .srmax = 0, + .srtau = 1.0 + } + }; AP_Float _thr_mix_man; // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle) AP_Float _thr_mix_min; // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)