Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl_Multi: add rate PIDs

This commit is contained in:
Randy Mackay 2016-02-15 15:25:41 +09:00
parent 75042e5e27
commit 5edc16dfb4
2 changed files with 168 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -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
View File

@ -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;
};