zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_AttControl_Multi: add rate PIDs

mission-4.1.18
Randy Mackay 9 years ago
parent
75042e5e27
commit
5edc16dfb4
2 changed files with 168 additions and 9 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. 123
      libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
  2. 54
      libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

123
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
Unescape View File

@ -4,6 +4,129 @@ @@ -4,6 +4,129 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
// 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
{

54
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
Unescape View File

@ -7,28 +7,64 @@ @@ -7,28 +7,64 @@
#include "AC_AttitudeControl.h"
#include <AP_Motors/AP_MotorsMulticopter.h>
// 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;
};

Write Preview
Loading…
Cancel
Save