diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index c9966cbefd91a..55359a1edb6fb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -19,8 +19,8 @@ #ifndef AC_ATC_MULTI_RATE_RP_IMAX # define AC_ATC_MULTI_RATE_RP_IMAX 0.5f #endif -#ifndef AC_ATC_MULTI_RATE_RP_FILT_HZ - # define AC_ATC_MULTI_RATE_RP_FILT_HZ 20.0f +#ifndef AC_ATC_MULTI_RATE_RPY_FILT_HZ + # define AC_ATC_MULTI_RATE_RPY_FILT_HZ 20.0f #endif #ifndef AC_ATC_MULTI_RATE_YAW_P # define AC_ATC_MULTI_RATE_YAW_P 0.180f @@ -106,9 +106,9 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { .d = AC_ATC_MULTI_RATE_RP_D, .ff = 0.0f, .imax = AC_ATC_MULTI_RATE_RP_IMAX, - .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .filt_E_hz = 0.0f, - .filt_D_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .srmax = 0, .srtau = 1.0 } @@ -120,9 +120,9 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { .d = AC_ATC_MULTI_RATE_RP_D, .ff = 0.0f, .imax = AC_ATC_MULTI_RATE_RP_IMAX, - .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .filt_E_hz = 0.0f, - .filt_D_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .srmax = 0, .srtau = 1.0 } @@ -135,9 +135,9 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { .d = AC_ATC_MULTI_RATE_YAW_D, .ff = 0.0f, .imax = AC_ATC_MULTI_RATE_YAW_IMAX, - .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .filt_E_hz = AC_ATC_MULTI_RATE_YAW_FILT_HZ, - .filt_D_hz = 0.0f, + .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ, .srmax = 0, .srtau = 1.0 } diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp index 5535d44990940..f8d2d76780e56 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -321,9 +321,9 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_V _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), - _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f), - _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f), - _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f) + _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f), + _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f), + _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f) { _dt = dt; AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_CustomControl/README.md b/libraries/AC_CustomControl/README.md index f1cf85fa10ce4..2d2d4a892d135 100644 --- a/libraries/AC_CustomControl/README.md +++ b/libraries/AC_CustomControl/README.md @@ -243,9 +243,9 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), - _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt), - _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt), - _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt) + _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt), + _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt), + _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt) { AP_Param::setup_object_defaults(this, var_info); }