Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: seperate kinimatic shaping from pid limit setting

Browse Source
This commit is contained in:
Leonard Hall 2021-07-08 13:43:05 +09:30 committed by Randy Mackay
parent 72e23cfb8c
commit 5f7607bbf0
2 changed files with 45 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -393,7 +393,10 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
/// Lateral position controller
///
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit
/// 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 can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic shaping.
void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
{
// return immediately if no change
@ -403,8 +406,6 @@ 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;
_p_pos_xy.set_limits(_vel_max_xy_cms, _accel_max_xy_cmss, 0.0f);
// 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());
@ -415,6 +416,13 @@ void AC_PosControl::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
/// This should be done only during initialisation to avoid discontinuities
void AC_PosControl::set_correction_speed_accel_xy(float speed_cms, float accel_cmss)
{
_p_pos_xy.set_limits(speed_cms, accel_cmss, 0.0f);
}
/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void AC_PosControl::init_xy_controller()
@ -646,8 +654,11 @@ void AC_PosControl::update_xy_controller()
/// Vertical position controller
///
/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit
/// speed_down can be positive or negative but will always be interpreted as a descent speed
/// 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 can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic shaping.
void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss)
{
// ensure speed_down is always negative
@ -668,8 +679,6 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa
if (is_positive(accel_cmss)) {
_accel_max_z_cmss = accel_cmss;
}
// define maximum position error and maximum first and second differential limits
_p_pos_z.set_limits(-fabsf(_vel_max_down_cms), _vel_max_up_cms, _accel_max_z_cmss, 0.0f);
// ensure the vertical time constant is not less than the filters in the _pid_accel_z object
_tc_z_s = _shaping_tc_z_s;
@ -681,6 +690,15 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa
}
}
/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
/// speed_down can be positive or negative but will always be interpreted as a descent speed.
/// This should be done only during initialisation to avoid discontinuities
void AC_PosControl::set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss)
{
// define maximum position error and maximum first and second differential limits
_p_pos_z.set_limits(-fabsf(speed_down), _vel_max_up_cms, _accel_max_z_cmss, 0.0f);
}
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void AC_PosControl::init_z_controller()
@ -1165,7 +1183,7 @@ bool AC_PosControl::calculate_yaw_and_rate_yaw()
}
}
// update the target yaw if origin and destination are at least 2m apart horizontally
// update the target yaw if velocity is greater than 5% _vel_max_xy_cms
if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) {
_yaw_target = degrees(vel_desired_xy.angle()) * 100.0f;
_yaw_rate_target = turn_rate*degrees(100.0f);

24
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -64,8 +64,15 @@ 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 can be done at any time as changes in these parameters are handled smoothly
/// by the kinimatic 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
/// This should be done only during initialisation to avoid discontinuities
void set_correction_speed_accel_xy(float speed_cms, float accel_cmss);
/// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s
float get_max_speed_xy_cms() const { return _vel_max_xy_cms; }
@ -129,8 +136,15 @@ 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.
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
/// speed_down can be positive or negative but will always be interpreted as a descent speed
/// This should be done only during initialisation to avoid discontinuities
void set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
/// get_max_accel_z_cmss - get the maximum vertical acceleration in cm/s/s
float get_max_accel_z_cmss() const { return _accel_max_z_cmss; }
@ -401,11 +415,11 @@ protected:
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 _vel_max_xy_cms; // max horizontal speed in cm/s
float _vel_max_up_cms; // max climb rate in cm/s
float _vel_max_down_cms; // max descent rate in cm/s
float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s
float _accel_max_z_cmss; // max vertical acceleration in cm/s/s
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
float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s used for kinematic shaping
float _accel_max_z_cmss; // max vertical acceleration in cm/s/s used for kinematic shaping
float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis
// output from controller