Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl_Heli: add rate PIDs

Browse Source
This commit is contained in:
Randy Mackay 2016-02-15 15:25:55 +09:00
parent 5edc16dfb4
commit 7f2c1f830f
2 changed files with 137 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -23,6 +23,111 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("HOVR_ROL_TRM", 1, AC_AttitudeControl_Heli, _hover_roll_trim, AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT), 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 AP_GROUPEND
}; };

38
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -9,6 +9,20 @@
#include <AC_PID/AC_HELI_PID.h> #include <AC_PID/AC_HELI_PID.h>
#include <Filter/Filter.h> #include <Filter/Filter.h>
// 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_INTEGRATOR_LEAK_RATE 0.02f
#define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 10.0f #define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 10.0f
#define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f #define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f
@ -20,13 +34,12 @@ public:
AC_AttitudeControl_Heli( AP_AHRS &ahrs, AC_AttitudeControl_Heli( AP_AHRS &ahrs,
const AP_Vehicle::MultiCopter &aparm, const AP_Vehicle::MultiCopter &aparm,
AP_MotorsHeli& motors, AP_MotorsHeli& motors,
AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw, float dt) :
AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw AC_AttitudeControl(ahrs, aparm, motors, dt),
) :
AC_AttitudeControl(ahrs, aparm, motors,
p_angle_roll, p_angle_pitch, p_angle_yaw,
pid_rate_roll, pid_rate_pitch, pid_rate_yaw),
_passthrough_roll(0), _passthrough_pitch(0), _passthrough_yaw(0), _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), pitch_feedforward_filter(AC_ATTITUDE_HELI_RATE_RP_FF_FILTER),
roll_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), yaw_velocity_feedforward_filter(AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER),
@ -44,6 +57,16 @@ public:
_flags_heli.do_piro_comp = false; _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 // 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); void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds);
@ -121,6 +144,9 @@ private:
// parameters // parameters
AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode 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 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. // LPF filters to act on Rate Feedforward terms to linearize output.
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead // Due to complicated aerodynamic effects, feedforwards acting too fast can lead