diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index f7fb45655e..f63de8e44f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -23,6 +23,111 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { // @User: Advanced AP_GROUPINFO("HOVR_ROL_TRM", 1, AC_AttitudeControl_Heli, _hover_roll_trim, AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT), + // @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_", 2, AC_AttitudeControl_Heli, AC_HELI_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_", 3, AC_AttitudeControl_Heli, AC_HELI_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_", 4, AC_AttitudeControl_Heli, AC_HELI_PID), + AP_GROUPEND }; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index d738ab740d..b8ee5751f9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -9,6 +9,20 @@ #include #include +// default rate controller PID gains +#define AC_ATC_HELI_RATE_RP_P 0.02 +#define AC_ATC_HELI_RATE_RP_I 0.5 +#define AC_ATC_HELI_RATE_RP_D 0.001 +#define AC_ATC_HELI_RATE_RP_IMAX 4500 +#define AC_ATC_HELI_RATE_RP_FF 0.05 +#define AC_ATC_HELI_RATE_RP_FILT_HZ 20.0f +#define AC_ATC_HELI_RATE_YAW_P 0.15 +#define AC_ATC_HELI_RATE_YAW_I 0.100 +#define AC_ATC_HELI_RATE_YAW_D 0.003 +#define AC_ATC_HELI_RATE_YAW_IMAX 4500 +#define AC_ATC_HELI_RATE_YAW_FF 0.02 +#define AC_ATC_HELI_RATE_YAW_FILT_HZ 20.0f + #define AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE 0.02f #define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 10.0f #define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f @@ -20,13 +34,12 @@ public: AC_AttitudeControl_Heli( AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsHeli& motors, - AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw, - AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw - ) : - AC_AttitudeControl(ahrs, aparm, motors, - p_angle_roll, p_angle_pitch, p_angle_yaw, - pid_rate_roll, pid_rate_pitch, pid_rate_yaw), + float dt) : + AC_AttitudeControl(ahrs, aparm, motors, dt), _passthrough_roll(0), _passthrough_pitch(0), _passthrough_yaw(0), + _pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_IMAX, AC_ATC_HELI_RATE_RP_FILT_HZ, dt, AC_ATC_HELI_RATE_RP_FF), + _pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_IMAX, AC_ATC_HELI_RATE_RP_FILT_HZ, dt, AC_ATC_HELI_RATE_RP_FF), + _pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATC_HELI_RATE_YAW_FILT_HZ, dt, AC_ATC_HELI_RATE_YAW_FF), pitch_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER), roll_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER), yaw_velocity_feedforward_filter(AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER), @@ -44,6 +57,16 @@ public: _flags_heli.do_piro_comp = false; } + // 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; } + + // same as above but allows accessing heli specific pid methods - used only by Copter's tuning.cpp + AC_HELI_PID& get_heli_rate_roll_pid() { return _pid_rate_roll; } + AC_HELI_PID& get_heli_rate_pitch_pid() { return _pid_rate_pitch; } + AC_HELI_PID& get_heli_rate_yaw_pid() { return _pid_rate_yaw; } + // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds); @@ -121,6 +144,9 @@ private: // parameters AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int16 _hover_roll_trim; // Angle in centi-degrees used to counter tail rotor thrust in hover + AC_HELI_PID _pid_rate_roll; + AC_HELI_PID _pid_rate_pitch; + AC_HELI_PID _pid_rate_yaw; // LPF filters to act on Rate Feedforward terms to linearize output. // Due to complicated aerodynamic effects, feedforwards acting too fast can lead