mirror of https://github.com/ArduPilot/ardupilot
409 lines
17 KiB
C++
409 lines
17 KiB
C++
#include "AC_CustomControl_config.h"
|
|
|
|
#if AP_CUSTOMCONTROL_PID_ENABLED
|
|
|
|
#include "AC_CustomControl_PID.h"
|
|
#include "AC_AttitudeControl/AC_AttitudeControl_Multi.h"
|
|
|
|
// 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
|
|
// @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
|
|
// @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
|
|
|
|
// @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
|
|
|
|
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
|
|
// @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
|
|
// @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
|
|
|
|
// @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
|
|
|
|
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
|
|
// @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
|
|
// @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
|
|
|
|
// @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
|
|
|
|
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*& 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_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);
|
|
}
|
|
|
|
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);
|
|
|
|
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
|
|
}
|
|
|
|
#endif // AP_CUSTOMCONTROL_PID_ENABLED
|