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

AC_AttitudeControl: AC_PosControl: support Control: Refactor to use Jerk

This commit is contained in:
Leonard Hall 2021-08-06 19:10:56 +09:30 committed by Randy Mackay
parent b25dab9c9b
commit f5dd8a9982
4 changed files with 116 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -270,23 +270,25 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max, 0.0f),
// @Param: _TC_XY
// @DisplayName: Time constant for the horizontal kinimatic input shaping
// @Description: Time constant of the horizontal kinimatic path generation used to determine how quickly the aircraft varies the acceleration target
// @Units: s
// @Range: 0.25 2
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("_TC_XY", 8, AC_PosControl, _shaping_tc_xy_s, POSCONTROL_DEFAULT_SHAPER_TC),
// IDs 8,9 used for _TC_XY and _TC_Z in beta release candidate
// @Param: _TC_Z
// @DisplayName: Time constant for the vertical kinimatic input shaping
// @Description: Time constant of the vertical kinimatic path generation used to determine how quickly the aircraft varies the acceleration target
// @Units: s
// @Range: 0.1 1
// @Increment: 0.01
// @Param: _JERK_XY
// @DisplayName: Jerk limit for the horizontal kinematic input shaping
// @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target
// @Units: m/s/s/s
// @Range: 1 20
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("_TC_Z", 9, AC_PosControl, _shaping_tc_z_s, POSCONTROL_DEFAULT_SHAPER_TC),
AP_GROUPINFO("_JERK_XY", 10, AC_PosControl, _shaping_jerk_xy, POSCONTROL_JERK_XY),
// @Param: _JERK_Z
// @DisplayName: Jerk limit for the vertical kinematic input shaping
// @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target
// @Units: m/s/s/s
// @Range: 5 50
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("_JERK_Z", 11, AC_PosControl, _shaping_jerk_z, POSCONTROL_JERK_Z),
AP_GROUPEND
};
@ -312,8 +314,8 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
_vel_max_xy_cms(POSCONTROL_SPEED),
_accel_max_z_cmss(POSCONTROL_ACCEL_Z),
_accel_max_xy_cmss(POSCONTROL_ACCEL_XY),
_tc_xy_s(POSCONTROL_DEFAULT_SHAPER_TC),
_tc_z_s(POSCONTROL_DEFAULT_SHAPER_TC)
_jerk_xy_max(POSCONTROL_JERK_XY),
_jerk_z_max(POSCONTROL_JERK_Z)
{
AP_Param::setup_object_defaults(this, var_info);
@ -330,9 +332,9 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
/// The kinematic path is constrained by the maximum jerk parameter and the velocity and acceleration limits set using the function set_max_speed_accel_xy.
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The jerk limit also defines the time taken to achieve the maximum acceleration.
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer)
{
@ -377,14 +379,14 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float
Vector2f vel;
Vector2f accel;
shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt);
vel_max_xy_cms, _accel_max_xy_cmss, _jerk_xy_max, _dt, false);
float posz = pos.z;
shape_pos_vel_accel(posz, 0, 0,
_pos_target.z, _vel_desired.z, _accel_desired.z,
vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms,
-vel_max_z_cms, vel_max_z_cms,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt);
_jerk_z_max, _dt, false);
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(pos_offset_z);
@ -412,9 +414,9 @@ float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_
///
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
/// This function only needs to be called if using the kinimatic shaping.
/// This function only needs to be called if using the kinematic shaping.
/// This can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic shaping.
/// by the kinematic shaping.
void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
{
// return immediately if no change
@ -424,13 +426,21 @@ void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
_vel_max_xy_cms = speed_cms;
_accel_max_xy_cmss = accel_cmss;
// ensure the horizontal time constant is not less than the vehicle is capable of
const float lean_angle = _accel_max_xy_cmss / (GRAVITY_MSS * 100.0 * M_PI / 18000.0);
const float angle_accel = MIN(_attitude_control.get_accel_pitch_max(), _attitude_control.get_accel_roll_max());
if (is_positive(angle_accel)) {
_tc_xy_s = MAX(_shaping_tc_xy_s, 2.0 * sqrtf(lean_angle / angle_accel));
} else {
_tc_xy_s = _shaping_tc_xy_s;
// ensure the horizontal jerk is less than the vehicle is capable of
const float jerk_max_cmsss = MIN(_attitude_control.get_ang_vel_roll_max_rads(), _attitude_control.get_ang_vel_pitch_max_rads()) * GRAVITY_MSS * 100.0;
const float snap_max_cmssss = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS * 100.0;
// get specified jerk limit
_jerk_xy_max = _shaping_jerk_xy * 100.0;
// limit maximum jerk based on maximum angular rate
if (is_positive(jerk_max_cmsss) && _attitude_control.get_bf_feedforward()) {
_jerk_xy_max = MIN(_jerk_xy_max, jerk_max_cmsss);
}
// limit maximum jerk to maximum possible average jerk based on angular acceleration
if (is_positive(snap_max_cmssss) && _attitude_control.get_bf_feedforward()) {
_jerk_xy_max = MIN(0.5 * safe_sqrt(_accel_max_xy_cmss * snap_max_cmssss), _jerk_xy_max);
}
}
@ -454,7 +464,7 @@ void AC_PosControl::init_xy_controller()
}
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinimatic path assumes a stationary initial condition but does not specify a specific starting position.
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void AC_PosControl::init_xy_controller_stopping_point()
{
@ -522,28 +532,29 @@ void AC_PosControl::init_xy()
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The jerk limit also defines the time taken to achieve the maximum acceleration.
void AC_PosControl::input_accel_xy(const Vector3f& accel)
{
// check for ekf xy position reset
handle_ekf_xy_reset();
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
shape_accel_xy(accel, _accel_desired, _accel_max_xy_cmss, _tc_xy_s, _dt);
shape_accel_xy(accel, _accel_desired, _jerk_xy_max, _dt);
}
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
/// The vel is projected forwards in time based on a time step of dt and acceleration accel.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel)
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output)
{
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(),
_vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt);
_accel_max_xy_cmss, _jerk_xy_max, _dt, limit_output);
update_vel_accel_xy(vel, accel, _dt, Vector2f());
}
@ -552,12 +563,13 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel)
/// The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The function alters the pos and vel to be the kinematic path based on accel
void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel)
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output)
{
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
_vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt);
_vel_max_xy_cms, _accel_max_xy_cmss, _jerk_xy_max, _dt, limit_output);
update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f());
}
@ -674,9 +686,9 @@ void AC_PosControl::update_xy_controller()
/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
/// speed_down can be positive or negative but will always be interpreted as a descent speed.
/// This function only needs to be called if using the kinimatic shaping.
/// This function only needs to be called if using the kinematic shaping.
/// This can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic shaping.
/// by the kinematic shaping.
void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss)
{
// ensure speed_down is always negative
@ -698,13 +710,13 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa
_accel_max_z_cmss = accel_cmss;
}
// ensure the vertical time constant is not less than the filters in the _pid_accel_z object
_tc_z_s = _shaping_tc_z_s;
// ensure the vertical Jerk is not limited by the filters in the Z accel PID object
_jerk_z_max = _shaping_jerk_z * 100.0;
if (is_positive(_pid_accel_z.filt_T_hz())) {
_tc_z_s = MAX(_tc_z_s, 2.0f/(M_2PI*_pid_accel_z.filt_T_hz()));
_jerk_z_max = MIN(_jerk_z_max, MIN(GRAVITY_MSS * 100.0, _accel_max_z_cmss) * (M_2PI * _pid_accel_z.filt_T_hz()) / 5.0);
}
if (is_positive(_pid_accel_z.filt_E_hz())) {
_tc_z_s = MAX(_tc_z_s, 2.0f/(M_2PI*_pid_accel_z.filt_E_hz()));
_jerk_z_max = MIN(_jerk_z_max, MIN(GRAVITY_MSS * 100.0, _accel_max_z_cmss) * (M_2PI * _pid_accel_z.filt_E_hz()) / 5.0);
}
}
@ -744,7 +756,7 @@ void AC_PosControl::init_z_controller_no_descent()
}
/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinimatic path assumes a stationary initial condition but does not specify a specific starting position.
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void AC_PosControl::init_z_controller_stopping_point()
{
@ -822,15 +834,14 @@ void AC_PosControl::input_accel_z(const float accel)
// adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z);
shape_accel(accel, _accel_desired.z,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt);
shape_accel(accel, _accel_desired.z, _jerk_z_max, _dt);
}
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant.
void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit)
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit, bool limit_output)
{
if (ignore_descent_limit) {
// turn off limits in the negative z direction
@ -851,15 +862,14 @@ void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore
shape_vel_accel(vel, accel,
_vel_desired.z, _accel_desired.z,
_vel_max_down_cms, _vel_max_up_cms,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt);
_jerk_z_max, _dt, limit_output);
update_vel_accel(vel, accel, _dt, 0);
}
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
/// using the default position control kinimatic path.
/// using the default position control kinematic path.
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel, bool ignore_descent_limit)
{
@ -871,7 +881,8 @@ void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel, bool ig
/// The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The function alters the pos and vel to be the kinematic path based on accel
void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float accel)
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float accel, bool limit_output)
{
// calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss;
@ -887,9 +898,9 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac
shape_pos_vel_accel(pos, vel, accel,
_pos_target.z, _vel_desired.z, _accel_desired.z,
0.0f, _vel_max_down_cms, _vel_max_up_cms,
_vel_max_down_cms, _vel_max_up_cms,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_tc_z_s, _dt);
_jerk_z_max, _dt, limit_output);
postype_t posp = pos;
update_pos_vel_accel(posp, vel, accel, _dt, 0);
@ -897,7 +908,7 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac
}
/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
/// using the default position control kinimatic path.
/// using the default position control kinematic path.
void AC_PosControl::set_alt_target_with_slew(const float& pos)
{
float posf = pos;
@ -916,8 +927,9 @@ void AC_PosControl::update_pos_offset_z(float pos_offset_z)
// input shape the terrain offset
shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f,
_pos_offset_z, _vel_offset_z, _accel_offset_z,
0.0f, get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(), _tc_z_s, _dt);
get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(),
_jerk_z_max, _dt, false);
}
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times

62
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -18,6 +18,8 @@
// position controller default definitions
#define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
#define POSCONTROL_JERK_XY 5.0f // default horizontal jerk m/s/s/s
#define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing
#define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending
@ -26,11 +28,10 @@
#define POSCONTROL_SPEED_UP 250.0f // default climb rate in cm/s
#define POSCONTROL_ACCEL_Z 250.0f // default vertical acceleration in cm/s/s.
#define POSCONTROL_JERK_Z 5.0f // default vertical jerk m/s/s/s
#define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz)
#define POSCONTROL_DEFAULT_SHAPER_TC 0.25f // default time constant of the kinimatic path generation in seconds
#define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
#define POSCONTROL_RELAX_TC 0.16f // This is used to decay the relevant variable to 5% in half a second.
@ -46,11 +47,8 @@ public:
/// get_dt - gets time delta in seconds for all position controllers
float get_dt() const { return _dt; }
/// get_shaping_tc_xy_s - gets the time constant of the xy kinimatic path generation in seconds
float get_shaping_tc_xy_s() const { return _shaping_tc_xy_s; }
/// get_shaping_tc_z_s - gets the time constant of the z kinimatic path generation in seconds
float get_shaping_tc_z_s() const { return _shaping_tc_z_s; }
/// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s
float get_shaping_jerk_xy_cmsss() const { return _shaping_jerk_xy*100.0; }
///
@ -59,7 +57,7 @@ public:
/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
void input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer);
/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
@ -70,9 +68,9 @@ public:
///
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
/// This function only needs to be called if using the kinimatic shaping.
/// This function only needs to be called if using the kinematic shaping.
/// This can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic shaping.
/// by the kinematic shaping.
void set_max_speed_accel_xy(float speed_cms, float accel_cmss);
/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
@ -94,7 +92,7 @@ public:
void init_xy_controller();
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinimatic path assumes a stationary initial condition but does not specify a specific starting position.
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void init_xy_controller_stopping_point();
@ -104,22 +102,24 @@ public:
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The jerk limit also defines the time taken to achieve the maximum acceleration.
void input_accel_xy(const Vector3f& accel);
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
/// The function alters the vel to be the kinematic path based on accel
void input_vel_accel_xy(Vector2f& vel, const Vector2f& accel);
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output = true);
/// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
/// The function alters the pos and vel to be the kinematic path based on accel
void input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel);
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output = true);
// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool is_active_xy() const;
@ -143,7 +143,7 @@ public:
/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
/// speed_down can be positive or negative but will always be interpreted as a descent speed
/// This can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic shaping.
/// by the kinematic shaping.
void set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
@ -176,7 +176,7 @@ public:
void init_z_controller_no_descent();
/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinimatic path assumes a stationary initial condition but does not specify a specific starting position.
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void init_z_controller_stopping_point();
@ -186,27 +186,29 @@ public:
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
virtual void input_accel_z(const float accel);
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
/// The function alters the vel to be the kinematic path based on accel
virtual void input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit);
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
virtual void input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit, bool limit_output = true);
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a climb rate in cm/s
/// using the default position control kinimatic path.
/// using the default position control kinematic path.
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void set_pos_target_z_from_climb_rate_cm(const float vel, bool ignore_descent_limit);
/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The function alters the pos and vel to be the kinematic path based on accel
void input_pos_vel_accel_z(float &pos, float &vel, float accel);
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void input_pos_vel_accel_z(float &pos, float &vel, float accel, bool limit_output = true);
/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
/// using the default position control kinimatic path.
/// using the default position control kinematic path.
void set_alt_target_with_slew(const float& pos);
/// update_pos_offset_z - updates the vertical offsets used by terrain following
@ -424,8 +426,8 @@ protected:
// parameters
AP_Float _lean_angle_max; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
AP_Float _shaping_tc_xy_s; // time constant of the xy kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
AP_Float _shaping_tc_z_s; // time constant of the z kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
AP_Float _shaping_jerk_xy; // Jerk limit of the xy kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
AP_Float _shaping_jerk_z; // Jerk limit of the z kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
AC_P_2D _p_pos_xy; // XY axis position controller to convert distance error to desired velocity
AC_P_1D _p_pos_z; // Z axis position controller to convert altitude error to desired climb rate
AC_PID_2D _pid_vel_xy; // XY axis velocity controller to convert velocity error to desired acceleration
@ -436,8 +438,8 @@ protected:
float _dt; // time difference (in seconds) between calls from the main program
uint64_t _last_update_xy_us; // system time (in microseconds) since last update_xy_controller call
uint64_t _last_update_z_us; // system time (in microseconds) since last update_z_controller call
float _tc_xy_s; // time constant of the xy kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
float _tc_z_s; // time constant of the z kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
float _jerk_xy_max; // Jerk limit of the xy kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
float _jerk_z_max; // Jerk limit of the z kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
float _vel_max_xy_cms; // max horizontal speed in cm/s used for kinematic shaping
float _vel_max_up_cms; // max climb rate in cm/s used for kinematic shaping
float _vel_max_down_cms; // max descent rate in cm/s used for kinematic shaping

17
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
View File

@ -7,17 +7,11 @@ AC_PosControl_Sub::AC_PosControl_Sub(AP_AHRS_View& ahrs, const AP_InertialNav& i
_alt_min(0.0f)
{}
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by :
/// maximum velocity - vel_max,
/// maximum acceleration - accel_max,
/// time constant - tc.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
/// The time constant must be positive.
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool force_descend)
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool force_descend, bool limit_output)
{
// check for ekf z position reset
handle_ekf_z_reset();
@ -48,9 +42,8 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
shape_vel_accel(vel, accel,
_vel_desired.z, _accel_desired.z,
_vel_max_down_cms, _vel_max_up_cms,
-accel_z_cms, accel_z_cms,
_tc_z_s, _dt);
_jerk_xy_max, _dt, limit_output);
update_vel_accel(vel, accel, _dt, _limit_vector.z);
}

14
libraries/AC_AttitudeControl/AC_PosControl_Sub.h
View File

@ -19,17 +19,11 @@ public:
/// set to zero to disable limit
void set_alt_min(float alt) { _alt_min = alt; }
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by :
/// maximum velocity - vel_max,
/// maximum acceleration - accel_max,
/// time constant - tc.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration.
/// The time constant must be positive.
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void input_vel_accel_z(float &vel, float accel, bool force_descend) override;
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void input_vel_accel_z(float &vel, float accel, bool force_descend, bool limit_output = true) override;
private:
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence