Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

libraries:AC_CustomControl: correct error due to changes in PID interface

This commit is contained in:
xianglunkai 2023-06-13 13:23:05 +08:00 committed by Peter Barker
parent 5800e9d785
commit bd30f3ede9
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_CustomControl/AC_CustomControl_PID.cpp
View File

@ -239,10 +239,11 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_V
_p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
_p_angle_pitch2(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), _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_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, 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),
_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_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 = dt;
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -289,9 +290,9 @@ Vector3f AC_CustomControl_PID::update()
// run rate controller // run rate controller
Vector3f gyro_latest = _ahrs->get_gyro_latest(); Vector3f gyro_latest = _ahrs->get_gyro_latest();
Vector3f motor_out; Vector3f motor_out;
motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], 1); motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], _dt, true);
motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], 1); motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], _dt, true);
motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], 1); motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], _dt, true);
return motor_out; return motor_out;
} }

1
libraries/AC_CustomControl/AC_CustomControl_PID.h
View File

@ -26,6 +26,7 @@ public:
protected: protected:
// put controller related variable here // put controller related variable here
float _dt;
// angle P controller objects // angle P controller objects
AC_P _p_angle_roll2; AC_P _p_angle_roll2;