zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_AttControl_Heli: add rate PIDs

master
Randy Mackay 9 years ago
parent
5edc16dfb4
commit
7f2c1f830f
2 changed files with 137 additions and 6 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 105
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
  2. 38
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

105
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
Unescape View File

@ -23,6 +23,111 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = { @@ -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
};

38
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
Unescape View File

@ -9,6 +9,20 @@ @@ -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: @@ -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: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save