From 5edc16dfb4a37081e825c35ff005a015ee364c08 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Feb 2016 15:25:41 +0900 Subject: [PATCH] AC_AttControl_Multi: add rate PIDs --- .../AC_AttitudeControl_Multi.cpp | 123 ++++++++++++++++++ .../AC_AttitudeControl_Multi.h | 54 ++++++-- 2 files changed, 168 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 78cbeca43f..1beca27666 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -4,6 +4,129 @@ #include #include +// table of user settable parameters +const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { + // parameters from parent vehicle + AP_NESTEDGROUPINFO(AC_AttitudeControl, 0), + + // @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.08 0.30 + // @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 0.5 + // @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 4500 + // @Increment: 10 + // @Units: Percent*10 + // @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.001 0.02 + // @Increment: 0.001 + // @User: Standard + + // @Param: RAT_RLL_FILT + // @DisplayName: Roll axis rate conroller input frequency in Hz + // @Description: Roll axis rate conroller input frequency in Hz + // @Unit: Hz + AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, 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.08 0.30 + // @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 0.5 + // @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 4500 + // @Increment: 10 + // @Units: Percent*10 + // @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.001 0.02 + // @Increment: 0.001 + // @User: Standard + + // @Param: RAT_PIT_FILT + // @DisplayName: Pitch axis rate conroller input frequency in Hz + // @Description: Pitch axis rate conroller input frequency in Hz + // @Unit: Hz + AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, 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.150 0.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 0.05 + // @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 4500 + // @Increment: 10 + // @Units: Percent*10 + // @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_FILT + // @DisplayName: Yaw axis rate conroller input frequency in Hz + // @Description: Yaw axis rate conroller input frequency in Hz + // @Unit: Hz + AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID), + + AP_GROUPEND +}; + +AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) : + AC_AttitudeControl(ahrs, aparm, motors, dt), + _motors_multi(motors), + _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), + _pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), + _pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_YAW_FILT_HZ, dt) +{ + AP_Param::setup_object_defaults(this, var_info); +} + // get lean angle max for pilot input that prioritises altitude hold over lean angle float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 7c76762f8c..0ed5c4fd9d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -7,28 +7,64 @@ #include "AC_AttitudeControl.h" #include +// default rate controller PID gains +#ifndef AC_ATC_MULTI_RATE_RP_P + # define AC_ATC_MULTI_RATE_RP_P 0.150f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_I + # define AC_ATC_MULTI_RATE_RP_I 0.100f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_D + # define AC_ATC_MULTI_RATE_RP_D 0.004f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_IMAX + # define AC_ATC_MULTI_RATE_RP_IMAX 2000.0f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_FILT_HZ + # define AC_ATC_MULTI_RATE_RP_FILT_HZ 20.0f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_P + # define AC_ATC_MULTI_RATE_YAW_P 0.200f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_I + # define AC_ATC_MULTI_RATE_YAW_I 0.020f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_D + # define AC_ATC_MULTI_RATE_YAW_D 0.0f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_IMAX + # define AC_ATC_MULTI_RATE_YAW_IMAX 1000.0f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_FILT_HZ + # define AC_ATC_MULTI_RATE_YAW_FILT_HZ 5.0f +#endif + + class AC_AttitudeControl_Multi : public AC_AttitudeControl { public: - AC_AttitudeControl_Multi(AP_AHRS &ahrs, - const AP_Vehicle::MultiCopter &aparm, - AP_MotorsMulticopter& motors, - AC_P& pi_angle_roll, AC_P& pi_angle_pitch, AC_P& pi_angle_yaw, - AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw - ) : - AC_AttitudeControl(ahrs, aparm, motors, pi_angle_roll, pi_angle_pitch, pi_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw), - _motors_multi(motors) - {} + AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt); // empty destructor to suppress compiler warning virtual ~AC_AttitudeControl_Multi() {} + // pid accessors + AC_PID& get_rate_roll_pid() { return _pid_rate_roll; } + AC_PID& get_rate_pitch_pid() { return _pid_rate_pitch; } + AC_PID& get_rate_yaw_pid() { return _pid_rate_yaw; } + // get lean angle max for pilot input that prioritises altitude hold over lean angle float get_althold_lean_angle_max() const; // calculate total body frame throttle required to produce the given earth frame throttle float get_boosted_throttle(float throttle_in); + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + protected: AP_MotorsMulticopter& _motors_multi; + AC_PID _pid_rate_roll; + AC_PID _pid_rate_pitch; + AC_PID _pid_rate_yaw; };