mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AC_AttControl_Heli: add rate PIDs
This commit is contained in:
Randy Mackay
parent
5edc16dfb4
commit
7f2c1f830f
@ -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
|
||||
};
|
||||
|
||||
|
||||
@ -9,6 +9,20 @@
|
||||
#include <AC_PID/AC_HELI_PID.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_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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user