diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index c0b4625491..147dedd198 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -7,13 +7,14 @@ #include "AC_CustomControl_Backend.h" // #include "AC_CustomControl_Empty.h" +#include "AC_CustomControl_PID.h" // table of user settable parameters const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // @Param: _TYPE // @DisplayName: Custom control type // @Description: Custom control type to be used - // @Values: 0:None, 1:Empty + // @Values: 0:None, 1:Empty, 2:PID // @RebootRequired: True // @User: Advanced AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE), @@ -28,6 +29,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // parameters for empty controller. only used as a template, no need for param table // AP_SUBGROUPVARPTR(_backend, "1_", 6, AC_CustomControl, _backend_var_info[0]), + // parameters for PID controller + AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]), + AP_GROUPEND }; @@ -53,6 +57,10 @@ void AC_CustomControl::init(void) // _backend = new AC_CustomControl_Empty(*this, _ahrs, _att_control, _motors, _dt); // _backend_var_info[get_type()] = AC_CustomControl_Empty::var_info; break; + case CustomControlType::CONT_PID: + _backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt); + _backend_var_info[get_type()] = AC_CustomControl_PID::var_info; + break; default: return; } diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index 3e92df5684..c11a51fb00 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -13,7 +13,7 @@ #if AP_CUSTOMCONTROL_ENABLED #ifndef CUSTOMCONTROL_MAX_TYPES -#define CUSTOMCONTROL_MAX_TYPES 1 +#define CUSTOMCONTROL_MAX_TYPES 2 #endif class AC_CustomControl_Backend; @@ -44,6 +44,7 @@ protected: enum class CustomControlType : uint8_t { CONT_NONE = 0, CONT_EMPTY = 1, + CONT_PID = 2, }; // controller that should be used enum class CustomControlOption { diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp new file mode 100644 index 0000000000..94b61bfc27 --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -0,0 +1,313 @@ +#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. Converts the difference between desired roll rate and actual roll rate into a motor speed output + // @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 motor output that the I gain 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 + 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. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output + // @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 motor output that the I gain 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 + 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. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output + // @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 motor output that the I gain 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 + 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, 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) +{ + 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], 1); + motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], 1); + motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], 1); + + 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(); +} + +#endif diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.h b/libraries/AC_CustomControl/AC_CustomControl_PID.h new file mode 100644 index 0000000000..9740061aad --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include +#include + +#include "AC_CustomControl_Backend.h" + +#ifndef CUSTOMCONTROL_PID_ENABLED + #define CUSTOMCONTROL_PID_ENABLED AP_CUSTOMCONTROL_ENABLED +#endif + +#if CUSTOMCONTROL_PID_ENABLED + +class AC_CustomControl_PID : public AC_CustomControl_Backend { +public: + AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt); + + // run lowest level body-frame rate controller and send outputs to the motors + Vector3f update() override; + void reset(void) override; + + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +protected: + // put controller related variable here + + // angle P controller objects + AC_P _p_angle_roll2; + AC_P _p_angle_pitch2; + AC_P _p_angle_yaw2; + + // rate PID controller objects + AC_PID _pid_atti_rate_roll; + AC_PID _pid_atti_rate_pitch; + AC_PID _pid_atti_rate_yaw; +}; + +#endif