mirror of https://github.com/ArduPilot/ardupilot
AC_CustomControl: add PID backend
This commit is contained in:
esaldiran
Peter Barker
parent
cbadf77652
commit
618164fea4
|
|
@ -7,13 +7,14 @@
|
|||
|
||||
#include "AC_CustomControl_Backend.h"
|
||||
// #include "AC_CustomControl_Empty.h"
|
||||
#include "AC_CustomControl_PID.h"
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo AC_CustomControl::var_info[] = {
|
||||
// @Param: _TYPE
|
||||
// @DisplayName: Custom control type
|
||||
// @Description: Custom control type to be used
|
||||
// @Values: 0:None, 1:Empty
|
||||
// @Values: 0:None, 1:Empty, 2:PID
|
||||
// @RebootRequired: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
|
@ -28,6 +29,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = {
|
|||
// parameters for empty controller. only used as a template, no need for param table
|
||||
// AP_SUBGROUPVARPTR(_backend, "1_", 6, AC_CustomControl, _backend_var_info[0]),
|
||||
|
||||
// parameters for PID controller
|
||||
AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -53,6 +57,10 @@ void AC_CustomControl::init(void)
|
|||
// _backend = new AC_CustomControl_Empty(*this, _ahrs, _att_control, _motors, _dt);
|
||||
// _backend_var_info[get_type()] = AC_CustomControl_Empty::var_info;
|
||||
break;
|
||||
case CustomControlType::CONT_PID:
|
||||
_backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt);
|
||||
_backend_var_info[get_type()] = AC_CustomControl_PID::var_info;
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@
|
|||
#if AP_CUSTOMCONTROL_ENABLED
|
||||
|
||||
#ifndef CUSTOMCONTROL_MAX_TYPES
|
||||
#define CUSTOMCONTROL_MAX_TYPES 1
|
||||
#define CUSTOMCONTROL_MAX_TYPES 2
|
||||
#endif
|
||||
|
||||
class AC_CustomControl_Backend;
|
||||
|
|
@ -44,6 +44,7 @@ protected:
|
|||
enum class CustomControlType : uint8_t {
|
||||
CONT_NONE = 0,
|
||||
CONT_EMPTY = 1,
|
||||
CONT_PID = 2,
|
||||
}; // controller that should be used
|
||||
|
||||
enum class CustomControlOption {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,313 @@
|
|||
#include "AC_CustomControl_PID.h"
|
||||
|
||||
#if CUSTOMCONTROL_PID_ENABLED
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = {
|
||||
// @Param: ANG_RLL_P
|
||||
// @DisplayName: Roll axis angle controller P gain
|
||||
// @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
|
||||
// @Range: 3.000 12.000
|
||||
// @Range{Sub}: 0.0 12.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_angle_roll2, "ANG_RLL_", 1, AC_CustomControl_PID, AC_P),
|
||||
|
||||
// @Param: ANG_PIT_P
|
||||
// @DisplayName: Pitch axis angle controller P gain
|
||||
// @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
|
||||
// @Range: 3.000 12.000
|
||||
// @Range{Sub}: 0.0 12.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_angle_pitch2, "ANG_PIT_", 2, AC_CustomControl_PID, AC_P),
|
||||
|
||||
// @Param: ANG_YAW_P
|
||||
// @DisplayName: Yaw axis angle controller P gain
|
||||
// @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
|
||||
// @Range: 3.000 12.000
|
||||
// @Range{Sub}: 0.0 6.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_angle_yaw2, "ANG_YAW_", 3, AC_CustomControl_PID, AC_P),
|
||||
|
||||
|
||||
// @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.01 0.5
|
||||
// @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 2.0
|
||||
// @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 1
|
||||
// @Increment: 0.01
|
||||
// @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.0 0.05
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_RLL_FF
|
||||
// @DisplayName: Roll axis rate controller feed forward
|
||||
// @Description: Roll axis rate controller feed forward
|
||||
// @Range: 0 0.5
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_RLL_FLTT
|
||||
// @DisplayName: Roll axis rate controller target frequency in Hz
|
||||
// @Description: Roll axis rate controller target frequency in Hz
|
||||
// @Range: 5 100
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_RLL_FLTE
|
||||
// @DisplayName: Roll axis rate controller error frequency in Hz
|
||||
// @Description: Roll axis rate controller error frequency in Hz
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_RLL_FLTD
|
||||
// @DisplayName: Roll axis rate controller derivative frequency in Hz
|
||||
// @Description: Roll axis rate controller derivative frequency in Hz
|
||||
// @Range: 5 100
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_RLL_SMAX
|
||||
// @DisplayName: Roll slew rate limit
|
||||
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
|
||||
// @Range: 0 200
|
||||
// @Increment: 0.5
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(_pid_atti_rate_roll, "RAT_RLL_", 4, AC_CustomControl_PID, 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.01 0.50
|
||||
// @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 2.0
|
||||
// @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 1
|
||||
// @Increment: 0.01
|
||||
// @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.0 0.05
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_PIT_FF
|
||||
// @DisplayName: Pitch axis rate controller feed forward
|
||||
// @Description: Pitch axis rate controller feed forward
|
||||
// @Range: 0 0.5
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_PIT_FLTT
|
||||
// @DisplayName: Pitch axis rate controller target frequency in Hz
|
||||
// @Description: Pitch axis rate controller target frequency in Hz
|
||||
// @Range: 5 100
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_PIT_FLTE
|
||||
// @DisplayName: Pitch axis rate controller error frequency in Hz
|
||||
// @Description: Pitch axis rate controller error frequency in Hz
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_PIT_FLTD
|
||||
// @DisplayName: Pitch axis rate controller derivative frequency in Hz
|
||||
// @Description: Pitch axis rate controller derivative frequency in Hz
|
||||
// @Range: 5 100
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_PIT_SMAX
|
||||
// @DisplayName: Pitch slew rate limit
|
||||
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
|
||||
// @Range: 0 200
|
||||
// @Increment: 0.5
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(_pid_atti_rate_pitch, "RAT_PIT_", 5, AC_CustomControl_PID, 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.10 2.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 1.0
|
||||
// @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 1
|
||||
// @Increment: 0.01
|
||||
// @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_FF
|
||||
// @DisplayName: Yaw axis rate controller feed forward
|
||||
// @Description: Yaw axis rate controller feed forward
|
||||
// @Range: 0 0.5
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_YAW_FLTT
|
||||
// @DisplayName: Yaw axis rate controller target frequency in Hz
|
||||
// @Description: Yaw axis rate controller target frequency in Hz
|
||||
// @Range: 1 50
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_YAW_FLTE
|
||||
// @DisplayName: Yaw axis rate controller error frequency in Hz
|
||||
// @Description: Yaw axis rate controller error frequency in Hz
|
||||
// @Range: 0 20
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_YAW_FLTD
|
||||
// @DisplayName: Yaw axis rate controller derivative frequency in Hz
|
||||
// @Description: Yaw axis rate controller derivative frequency in Hz
|
||||
// @Range: 5 50
|
||||
// @Increment: 1
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RAT_YAW_SMAX
|
||||
// @DisplayName: Yaw slew rate limit
|
||||
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
|
||||
// @Range: 0 200
|
||||
// @Increment: 0.5
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(_pid_atti_rate_yaw, "RAT_YAW_", 6, AC_CustomControl_PID, AC_PID),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) :
|
||||
AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt),
|
||||
_p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
|
||||
_p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
|
||||
_p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
|
||||
_pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt),
|
||||
_pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt),
|
||||
_pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
Vector3f AC_CustomControl_PID::update()
|
||||
{
|
||||
// reset controller based on spool state
|
||||
switch (_motors->get_spool_state()) {
|
||||
case AP_Motors::SpoolState::SHUT_DOWN:
|
||||
case AP_Motors::SpoolState::GROUND_IDLE:
|
||||
// We are still at the ground. Reset custom controller to avoid
|
||||
// build up, ex: integrator
|
||||
reset();
|
||||
break;
|
||||
|
||||
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
|
||||
case AP_Motors::SpoolState::SPOOLING_UP:
|
||||
case AP_Motors::SpoolState::SPOOLING_DOWN:
|
||||
// we are off the ground
|
||||
break;
|
||||
}
|
||||
|
||||
// run custom controller after here
|
||||
Quaternion attitude_body, attitude_target;
|
||||
_ahrs->get_quat_body_to_ned(attitude_body);
|
||||
|
||||
attitude_target = _att_control->get_attitude_target_quat();
|
||||
// This vector represents the angular error to rotate the thrust vector using x and y and heading using z
|
||||
Vector3f attitude_error;
|
||||
float _thrust_angle, _thrust_error_angle;
|
||||
_att_control->thrust_heading_rotation_angles(attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
|
||||
|
||||
// recalculate ang vel feedforward from attitude target model
|
||||
// rotation from the target frame to the body frame
|
||||
Quaternion rotation_target_to_body = attitude_body.inverse() * attitude_target;
|
||||
// target angle velocity vector in the body frame
|
||||
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _att_control->get_attitude_target_ang_vel();
|
||||
|
||||
// run attitude controller
|
||||
Vector3f target_rate;
|
||||
target_rate[0] = _p_angle_roll2.kP() * attitude_error.x + ang_vel_body_feedforward[0];
|
||||
target_rate[1] = _p_angle_pitch2.kP() * attitude_error.y + ang_vel_body_feedforward[1];
|
||||
target_rate[2] = _p_angle_yaw2.kP() * attitude_error.z + ang_vel_body_feedforward[2];
|
||||
|
||||
// run rate controller
|
||||
Vector3f gyro_latest = _ahrs->get_gyro_latest();
|
||||
Vector3f motor_out;
|
||||
motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], 1);
|
||||
motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], 1);
|
||||
motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], 1);
|
||||
|
||||
return motor_out;
|
||||
}
|
||||
|
||||
// This example uses exact same controller architecture as ArduCopter attitude controller without all the safe guard against saturation.
|
||||
// The gains are scaled 0.9 times to better detect switch over response.
|
||||
// Note that integrator are not reset correctly as it is done in reset_main_att_controller inside AC_CustomControl.cpp
|
||||
// This is done intentionally to demonstrate switch over performance of two exact controller with different reset handling.
|
||||
void AC_CustomControl_PID::reset(void)
|
||||
{
|
||||
_pid_atti_rate_roll.reset_I();
|
||||
_pid_atti_rate_pitch.reset_I();
|
||||
_pid_atti_rate_yaw.reset_I();
|
||||
_pid_atti_rate_roll.reset_filter();
|
||||
_pid_atti_rate_pitch.reset_filter();
|
||||
_pid_atti_rate_yaw.reset_filter();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AC_PID/AC_PID.h>
|
||||
#include <AC_PID/AC_P.h>
|
||||
|
||||
#include "AC_CustomControl_Backend.h"
|
||||
|
||||
#ifndef CUSTOMCONTROL_PID_ENABLED
|
||||
#define CUSTOMCONTROL_PID_ENABLED AP_CUSTOMCONTROL_ENABLED
|
||||
#endif
|
||||
|
||||
#if CUSTOMCONTROL_PID_ENABLED
|
||||
|
||||
class AC_CustomControl_PID : public AC_CustomControl_Backend {
|
||||
public:
|
||||
AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt);
|
||||
|
||||
// run lowest level body-frame rate controller and send outputs to the motors
|
||||
Vector3f update() override;
|
||||
void reset(void) override;
|
||||
|
||||
// user settable parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
// put controller related variable here
|
||||
|
||||
// angle P controller objects
|
||||
AC_P _p_angle_roll2;
|
||||
AC_P _p_angle_pitch2;
|
||||
AC_P _p_angle_yaw2;
|
||||
|
||||
// rate PID controller objects
|
||||
AC_PID _pid_atti_rate_roll;
|
||||
AC_PID _pid_atti_rate_pitch;
|
||||
AC_PID _pid_atti_rate_yaw;
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue