ardupilot/libraries/AC_CustomControl/AC_CustomControl_PID.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

406 lines
17 KiB
C++
Raw Normal View History

2022-08-04 21:26:39 -03:00
#include "AC_CustomControl_PID.h"
#if CUSTOMCONTROL_PID_ENABLED
// table of user settable parameters
const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = {
// @Param: ANG_RLL_P
// @DisplayName: Roll axis angle controller P gain
// @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
// @Range: 3.000 12.000
// @Range{Sub}: 0.0 12.000
// @User: Standard
AP_SUBGROUPINFO(_p_angle_roll2, "ANG_RLL_", 1, AC_CustomControl_PID, AC_P),
// @Param: ANG_PIT_P
// @DisplayName: Pitch axis angle controller P gain
// @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
// @Range: 3.000 12.000
// @Range{Sub}: 0.0 12.000
// @User: Standard
AP_SUBGROUPINFO(_p_angle_pitch2, "ANG_PIT_", 2, AC_CustomControl_PID, AC_P),
// @Param: ANG_YAW_P
// @DisplayName: Yaw axis angle controller P gain
// @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
// @Range: 3.000 12.000
// @Range{Sub}: 0.0 6.000
// @User: Standard
AP_SUBGROUPINFO(_p_angle_yaw2, "ANG_YAW_", 3, AC_CustomControl_PID, AC_P),
// @Param: RAT_RLL_P
// @DisplayName: Roll axis rate controller P gain
// @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
2022-08-04 21:26:39 -03:00
// @Range: 0.01 0.5
// @Increment: 0.005
// @User: Standard
// @Param: RAT_RLL_I
// @DisplayName: Roll axis rate controller I gain
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
// @Range: 0.01 2.0
// @Increment: 0.01
// @User: Standard
// @Param: RAT_RLL_IMAX
// @DisplayName: Roll axis rate controller I gain maximum
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
2022-08-04 21:26:39 -03:00
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: RAT_RLL_D
// @DisplayName: Roll axis rate controller D gain
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
// @Range: 0.0 0.05
// @Increment: 0.001
// @User: Standard
// @Param: RAT_RLL_FF
// @DisplayName: Roll axis rate controller feed forward
// @Description: Roll axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: RAT_RLL_FLTT
// @DisplayName: Roll axis rate controller target frequency in Hz
// @Description: Roll axis rate controller target frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTE
// @DisplayName: Roll axis rate controller error frequency in Hz
// @Description: Roll axis rate controller error frequency in Hz
// @Range: 0 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTD
// @DisplayName: Roll axis rate controller derivative frequency in Hz
// @Description: Roll axis rate controller derivative frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_SMAX
// @DisplayName: Roll slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2023-09-18 22:45:45 -03:00
// @Param: RAT_RLL_PDMX
// @DisplayName: Roll axis rate controller PD sum maximum
// @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
// @Range: 0 1
// @Increment: 0.01
// @User: Advanced
// @Param: RAT_RLL_D_FF
// @DisplayName: Roll Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
// @Param: RAT_RLL_NTF
// @DisplayName: Roll Target notch filter index
// @Description: Roll Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_RLL_NEF
// @DisplayName: Roll Error notch filter index
// @Description: Roll Error notch filter index
// @Range: 1 8
// @User: Advanced
2022-08-04 21:26:39 -03:00
AP_SUBGROUPINFO(_pid_atti_rate_roll, "RAT_RLL_", 4, AC_CustomControl_PID, AC_PID),
// @Param: RAT_PIT_P
// @DisplayName: Pitch axis rate controller P gain
// @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
2022-08-04 21:26:39 -03:00
// @Range: 0.01 0.50
// @Increment: 0.005
// @User: Standard
// @Param: RAT_PIT_I
// @DisplayName: Pitch axis rate controller I gain
// @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
// @Range: 0.01 2.0
// @Increment: 0.01
// @User: Standard
// @Param: RAT_PIT_IMAX
// @DisplayName: Pitch axis rate controller I gain maximum
// @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
2022-08-04 21:26:39 -03:00
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: RAT_PIT_D
// @DisplayName: Pitch axis rate controller D gain
// @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
// @Range: 0.0 0.05
// @Increment: 0.001
// @User: Standard
// @Param: RAT_PIT_FF
// @DisplayName: Pitch axis rate controller feed forward
// @Description: Pitch axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: RAT_PIT_FLTT
// @DisplayName: Pitch axis rate controller target frequency in Hz
// @Description: Pitch axis rate controller target frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTE
// @DisplayName: Pitch axis rate controller error frequency in Hz
// @Description: Pitch axis rate controller error frequency in Hz
// @Range: 0 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTD
// @DisplayName: Pitch axis rate controller derivative frequency in Hz
// @Description: Pitch axis rate controller derivative frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_SMAX
// @DisplayName: Pitch slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2023-09-18 22:45:45 -03:00
// @Param: RAT_PIT_PDMX
// @DisplayName: Pitch axis rate controller PD sum maximum
// @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
// @Range: 0 1
// @Increment: 0.01
// @User: Advanced
// @Param: RAT_PIT_D_FF
// @DisplayName: Pitch Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
// @Param: RAT_PIT_NTF
// @DisplayName: Pitch Target notch filter index
// @Description: Pitch Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_PIT_NEF
// @DisplayName: Pitch Error notch filter index
// @Description: Pitch Error notch filter index
// @Range: 1 8
// @User: Advanced
2022-08-04 21:26:39 -03:00
AP_SUBGROUPINFO(_pid_atti_rate_pitch, "RAT_PIT_", 5, AC_CustomControl_PID, AC_PID),
// @Param: RAT_YAW_P
// @DisplayName: Yaw axis rate controller P gain
// @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
2022-08-04 21:26:39 -03:00
// @Range: 0.10 2.50
// @Increment: 0.005
// @User: Standard
// @Param: RAT_YAW_I
// @DisplayName: Yaw axis rate controller I gain
// @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
// @Range: 0.010 1.0
// @Increment: 0.01
// @User: Standard
// @Param: RAT_YAW_IMAX
// @DisplayName: Yaw axis rate controller I gain maximum
// @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
2022-08-04 21:26:39 -03:00
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: RAT_YAW_D
// @DisplayName: Yaw axis rate controller D gain
// @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
// @Range: 0.000 0.02
// @Increment: 0.001
// @User: Standard
// @Param: RAT_YAW_FF
// @DisplayName: Yaw axis rate controller feed forward
// @Description: Yaw axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: RAT_YAW_FLTT
// @DisplayName: Yaw axis rate controller target frequency in Hz
// @Description: Yaw axis rate controller target frequency in Hz
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTE
// @DisplayName: Yaw axis rate controller error frequency in Hz
// @Description: Yaw axis rate controller error frequency in Hz
// @Range: 0 20
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTD
// @DisplayName: Yaw axis rate controller derivative frequency in Hz
// @Description: Yaw axis rate controller derivative frequency in Hz
// @Range: 5 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_SMAX
// @DisplayName: Yaw slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2023-09-18 22:45:45 -03:00
// @Param: RAT_YAW_PDMX
// @DisplayName: Yaw axis rate controller PD sum maximum
// @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
// @Range: 0 1
// @Increment: 0.01
// @User: Advanced
// @Param: RAT_YAW_D_FF
// @DisplayName: Yaw Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
// @Param: RAT_YAW_NTF
// @DisplayName: Yaw Target notch filter index
// @Description: Yaw Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_YAW_NEF
// @DisplayName: Yaw Error notch filter index
// @Description: Yaw Error notch filter index
// @Range: 1 8
// @User: Advanced
2022-08-04 21:26:39 -03:00
AP_SUBGROUPINFO(_pid_atti_rate_yaw, "RAT_YAW_", 6, AC_CustomControl_PID, AC_PID),
AP_GROUPEND
};
AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) :
AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt),
_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)
2022-08-04 21:26:39 -03:00
{
_dt = dt;
2022-08-04 21:26:39 -03:00
AP_Param::setup_object_defaults(this, var_info);
}
Vector3f AC_CustomControl_PID::update()
{
// reset controller based on spool state
switch (_motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
case AP_Motors::SpoolState::GROUND_IDLE:
// We are still at the ground. Reset custom controller to avoid
// build up, ex: integrator
reset();
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// we are off the ground
break;
}
// run custom controller after here
Quaternion attitude_body, attitude_target;
_ahrs->get_quat_body_to_ned(attitude_body);
attitude_target = _att_control->get_attitude_target_quat();
// This vector represents the angular error to rotate the thrust vector using x and y and heading using z
Vector3f attitude_error;
float _thrust_angle, _thrust_error_angle;
_att_control->thrust_heading_rotation_angles(attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
// recalculate ang vel feedforward from attitude target model
// rotation from the target frame to the body frame
Quaternion rotation_target_to_body = attitude_body.inverse() * attitude_target;
// target angle velocity vector in the body frame
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _att_control->get_attitude_target_ang_vel();
// run attitude controller
Vector3f target_rate;
target_rate[0] = _p_angle_roll2.kP() * attitude_error.x + ang_vel_body_feedforward[0];
target_rate[1] = _p_angle_pitch2.kP() * attitude_error.y + ang_vel_body_feedforward[1];
target_rate[2] = _p_angle_yaw2.kP() * attitude_error.z + ang_vel_body_feedforward[2];
// run rate controller
Vector3f gyro_latest = _ahrs->get_gyro_latest();
Vector3f motor_out;
motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], _dt, false);
motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], _dt, false);
motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], _dt, false);
2022-08-04 21:26:39 -03:00
return motor_out;
}
// This example uses exact same controller architecture as ArduCopter attitude controller without all the safe guard against saturation.
// The gains are scaled 0.9 times to better detect switch over response.
// Note that integrator are not reset correctly as it is done in reset_main_att_controller inside AC_CustomControl.cpp
// This is done intentionally to demonstrate switch over performance of two exact controller with different reset handling.
void AC_CustomControl_PID::reset(void)
{
_pid_atti_rate_roll.reset_I();
_pid_atti_rate_pitch.reset_I();
_pid_atti_rate_yaw.reset_I();
_pid_atti_rate_roll.reset_filter();
_pid_atti_rate_pitch.reset_filter();
_pid_atti_rate_yaw.reset_filter();
}
void AC_CustomControl_PID::set_notch_sample_rate(float sample_rate)
{
#if AP_FILTER_ENABLED
_pid_atti_rate_roll.set_notch_sample_rate(sample_rate);
_pid_atti_rate_pitch.set_notch_sample_rate(sample_rate);
_pid_atti_rate_yaw.set_notch_sample_rate(sample_rate);
#endif
}
2022-08-04 21:26:39 -03:00
#endif