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

AP_Math: Control: Add directional based acceleration limit 

AP_Math: Control: Add directional based acceleration limit
This commit is contained in:
Leonard Hall 2021-12-15 15:38:51 +10:30 committed by Randy Mackay
parent b34a7e46a8
commit 1e124ca957
2 changed files with 120 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
libraries/AP_Math/control.cpp
View File

@ -29,9 +29,9 @@
#define CORNER_ACCELERATION_RATIO 1.0/safe_sqrt(2.0) // acceleration reduction to enable zero overshoot corners #define CORNER_ACCELERATION_RATIO 1.0/safe_sqrt(2.0) // acceleration reduction to enable zero overshoot corners
// update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel. // update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel.
// the velocity is not moved in the direction of limit if limit is not set to zero // the velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// vel_error - specifies the direction of the velocity error useded in limit handling // vel_error - specifies the direction of the velocity error useded in limit handling.
void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error) void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error)
{ {
const float delta_vel = accel * dt; const float delta_vel = accel * dt;
@ -42,9 +42,9 @@ void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_
} }
// update_pos_vel_accel - single axis projection of position and velocity forward in time based on a time step of dt and acceleration of accel. // update_pos_vel_accel - single axis projection of position and velocity forward in time based on a time step of dt and acceleration of accel.
// the position and velocity is not moved in the direction of limit if limit is not set to zero // the position and velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling // pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error) void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error)
{ {
// move position and velocity forward by dt if it does not increase error when limited. // move position and velocity forward by dt if it does not increase error when limited.
@ -58,9 +58,9 @@ void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, flo
} }
// update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel. // update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the velocity is not moved in the direction of limit if limit is not set to zero // the velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling // pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error) void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error)
{ {
// increase velocity by acceleration * dt if it does not increase error when limited. // increase velocity by acceleration * dt if it does not increase error when limited.
@ -77,9 +77,9 @@ void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const V
} }
// update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel. // update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the position and velocity is not moved in the direction of limit if limit is not set to zero // the position and velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// pos_error and vel_error - specifies the direction of the velocity error used in limit handling // pos_error and vel_error - specifies the direction of the velocity error used in limit handling.
void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error) void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error)
{ {
// move position and velocity forward by dt. // move position and velocity forward by dt.
@ -100,12 +100,12 @@ void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration. /* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by : The kinematic path is constrained by :
acceleration limits - accel_min, accel_max, acceleration limits - accel_min, accel_max,
time constant - tc. time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. 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 also defines the time taken to achieve the maximum acceleration.
The time constant must be positive. The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to accel_input The function alters the variable accel to follow a jerk limited kinematic path to accel_input.
*/ */
void shape_accel(float accel_input, float& accel, void shape_accel(float accel_input, float& accel,
float jerk_max, float dt) float jerk_max, float dt)
@ -141,17 +141,16 @@ void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
accel.y = accel_2f.y; accel.y = accel_2f.y;
} }
/* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity. /* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
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 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 kinematic path is constrained by :
maximum velocity - vel_max, maximum velocity - vel_max,
maximum acceleration - accel_max, maximum acceleration - accel_max,
time constant - tc. time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. 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 also defines the time taken to achieve the maximum acceleration.
The time constant must be positive. The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input.
The accel_max limit can be removed by setting it to zero. The accel_max limit can be removed by setting it to zero.
*/ */
void shape_vel_accel(float vel_input, float accel_input, void shape_vel_accel(float vel_input, float accel_input,
@ -246,13 +245,13 @@ void shape_vel_accel_xy(const Vector2f &vel_input, const Vector2f& accel_input,
/* shape_pos_vel_accel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity. /* shape_pos_vel_accel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
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 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 kinematic path is constrained by :
maximum velocity - vel_max, maximum velocity - vel_max,
maximum acceleration - accel_max, maximum acceleration - accel_max,
time constant - tc. time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. 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 also defines the time taken to achieve the maximum acceleration.
The time constant must be positive. The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input.
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero. The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/ */
void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input, void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input,
@ -324,7 +323,45 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total_accel); shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total_accel);
} }
// proportional controller with piecewise sqrt sections to constrain second derivative /* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
Input parameters are:
vel is the velocity vector used to define the direction acceleration limit is biased in.
accel is the acceleration vector to be limited.
accel_max is the maximum length of the acceleration vector after being limited.
Returns true when accel vector has been limited.
*/
bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max)
{
// check accel_max is defined
if (!is_positive(accel_max)) {
return false;
}
// limit acceleration to accel_max while prioritizing cross track acceleration
if (accel.length_squared() > sq(accel_max)) {
if (vel.is_zero()) {
// We do not have a direction of travel so do a simple vector length limit
accel.limit_length(accel_max);
} else {
// calculate acceleration in the direction of and perpendicular to the velocity input
const Vector2f vel_input_unit = vel.normalized();
// acceleration in the direction of travel
float accel_dir = vel_input_unit * accel;
// cross track acceleration
Vector2f accel_cross = accel - (vel_input_unit * accel_dir);
if (accel_cross.limit_length(accel_max)) {
accel_dir = 0.0;
} else {
float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared());
accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir);
}
accel = accel_cross + vel_input_unit * accel_dir;
}
return true;
}
return false;
}
// sqrt_controller calculates the correction based on a proportional controller with piecewise sqrt sections to constrain second derivative.
float sqrt_controller(float error, float p, float second_ord_lim, float dt) float sqrt_controller(float error, float p, float second_ord_lim, float dt)
{ {
float correction_rate; float correction_rate;
@ -359,7 +396,7 @@ float sqrt_controller(float error, float p, float second_ord_lim, float dt)
} }
} }
// proportional controller with piecewise sqrt sections to constrain second derivative // sqrt_controller calculates the correction based on a proportional controller with piecewise sqrt sections to constrain second derivative.
Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt) Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt)
{ {
const float error_length = error.length(); const float error_length = error.length();
@ -371,7 +408,8 @@ Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, f
return error * (correction_length / error_length); return error * (correction_length / error_length);
} }
// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output // inv_sqrt_controller calculates the inverse of the sqrt controller.
// This function calculates the input (aka error) to the sqrt_controller required to achieve a given output.
float inv_sqrt_controller(float output, float p, float D_max) float inv_sqrt_controller(float output, float p, float D_max)
{ {
if (is_positive(D_max) && is_zero(p)) { if (is_positive(D_max) && is_zero(p)) {
@ -384,7 +422,7 @@ float inv_sqrt_controller(float output, float p, float D_max)
return 0.0; return 0.0;
} }
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function.
const float linear_velocity = D_max / p; const float linear_velocity = D_max / p;
if (fabsf(output) < linear_velocity) { if (fabsf(output) < linear_velocity) {
@ -397,13 +435,13 @@ float inv_sqrt_controller(float output, float p, float D_max)
return is_positive(output) ? stopping_dist : -stopping_dist; return is_positive(output) ? stopping_dist : -stopping_dist;
} }
// calculate the stopping distance for the square root controller based deceleration path // stopping_distance calculates the stopping distance for the square root controller based deceleration path.
float stopping_distance(float velocity, float p, float accel_max) float stopping_distance(float velocity, float p, float accel_max)
{ {
return inv_sqrt_controller(velocity, p, accel_max); return inv_sqrt_controller(velocity, p, accel_max);
} }
// calculate the maximum acceleration or velocity in a given direction // kinematic_limit calculates the maximum acceleration or velocity in a given direction.
// based on horizontal and vertical limits. // based on horizontal and vertical limits.
float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg) float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg)
{ {
@ -440,9 +478,9 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m
return fabsf(max_z_neg/direction.z); return fabsf(max_z_neg/direction.z);
} }
// calculate the expo function on the normalised input // input_expo calculates the expo function on the normalised input.
// input must be in the range of -1 to 1 // The input must be in the range of -1 to 1.
// expo should be less than 1.0 but limited to be less than 0.95 // The expo should be less than 1.0 but limited to be less than 0.95.
float input_expo(float input, float expo) float input_expo(float input, float expo)
{ {
input = constrain_float(input, -1.0, 1.0); input = constrain_float(input, -1.0, 1.0);

83
libraries/AP_Math/control.h
View File

@ -26,78 +26,82 @@ typedef Vector3f Vector3p;
*/ */
// update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel. // update_vel_accel - single axis projection of velocity, vel, forwards in time based on a time step of dt and acceleration of accel.
// the velocity is not moved in the direction of limit if limit is not set to zero // the velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// vel_error - specifies the direction of the velocity error useded in limit handling // vel_error - specifies the direction of the velocity error useded in limit handling.
void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error); void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error);
// update_pos_vel_accel - single axis projection of position and velocity forward in time based on a time step of dt and acceleration of accel. // update_pos_vel_accel - single axis projection of position and velocity forward in time based on a time step of dt and acceleration of accel.
// the position and velocity is not moved in the direction of limit if limit is not set to zero // the position and velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling // pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
void update_pos_vel_accel(postype_t & pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error); void update_pos_vel_accel(postype_t & pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error);
// update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel. // update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the velocity is not moved in the direction of limit if limit is not set to zero // the velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling // pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error); void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error);
// update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel. // update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// the position and velocity is not moved in the direction of limit if limit is not set to zero // the position and velocity is not moved in the direction of limit if limit is not set to zero.
// limit - specifies if the system is unable to continue to accelerate // limit - specifies if the system is unable to continue to accelerate.
// pos_error and vel_error - specifies the direction of the velocity error useded in limit handling // pos_error and vel_error - specifies the direction of the velocity error useded in limit handling.
void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error); void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error);
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration. /* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by : The kinematic path is constrained by :
acceleration limits - accel_min, accel_max, acceleration limits - accel_min, accel_max,
time constant - tc. time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. 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 also defines the time taken to achieve the maximum acceleration.
The time constant must be positive. The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to accel_input The function alters the variable accel to follow a jerk limited kinematic path to accel_input.
*/ */
void shape_accel(float accel_input, float& accel, void shape_accel(float accel_input, float& accel,
float jerk_max, float dt); float jerk_max, float dt);
// 2D version
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
float jerk_max, float dt); float jerk_max, float dt);
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel, void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
float jerk_max, float dt); float jerk_max, float dt);
/* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity. /* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. 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 kinematic path is constrained by :
velocity limits - vel_min, vel_max, maximum velocity - vel_max,
acceleration limits - accel_min, accel_max, maximum acceleration - accel_max,
time constant - tc. time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. 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 also defines the time taken to achieve the maximum acceleration.
The time constant must be positive. The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input.
The accel_max limit can be removed by setting it to zero.
*/ */
void shape_vel_accel(float vel_input, float accel_input, void shape_vel_accel(float vel_input, float accel_input,
float vel, float& accel, float vel, float& accel,
float accel_min, float accel_max, float accel_min, float accel_max,
float jerk_max, float dt, bool limit_total_accel); float jerk_max, float dt, bool limit_total_accel);
// 2D version
void shape_vel_accel_xy(const Vector2f &vel_input1, const Vector2f& accel_input, void shape_vel_accel_xy(const Vector2f &vel_input1, const Vector2f& accel_input,
const Vector2f& vel, Vector2f& accel, const Vector2f& vel, Vector2f& accel,
float accel_max, float jerk_max, float dt, bool limit_total_accel); float accel_max, float jerk_max, float dt, bool limit_total_accel);
/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity. /* shape_pos_vel_accel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
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 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 kinematic path is constrained by :
maximum velocity - vel_max, maximum velocity - vel_max,
maximum acceleration - accel_max, maximum acceleration - accel_max,
time constant - tc. time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. 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 also defines the time taken to achieve the maximum acceleration.
The time constant must be positive. The time constant must be positive.
The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input.
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/ */
void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel_input, void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel_input,
const postype_t pos, float vel, float& accel, const postype_t pos, float vel, float& accel,
@ -105,28 +109,39 @@ void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel
float accel_min, float accel_max, float accel_min, float accel_max,
float jerk_max, float dt, bool limit_total_accel); float jerk_max, float dt, bool limit_total_accel);
// 2D version
void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2p& pos, const Vector2f& vel, Vector2f& accel, const Vector2p& pos, const Vector2f& vel, Vector2f& accel,
float vel_max, float accel_max, float vel_max, float accel_max,
float jerk_max, float dt, bool limit_total_accel); float jerk_max, float dt, bool limit_total_accel);
// proportional controller with piecewise sqrt sections to constrain second derivative /* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
Input parameters are:
vel is the velocity vector used to define the direction acceleration limit is biased in.
accel is the acceleration vector to be limited.
accel_max is the maximum length of the acceleration vector after being limited.
Returns true when accel vector has been limited.
*/
bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max);
// sqrt_controller calculates the correction based on a proportional controller with piecewise sqrt sections to constrain second derivative.
float sqrt_controller(float error, float p, float second_ord_lim, float dt); float sqrt_controller(float error, float p, float second_ord_lim, float dt);
// proportional controller with piecewise sqrt sections to constrain second derivative // sqrt_controller calculates the correction based on a proportional controller with piecewise sqrt sections to constrain second derivative.
Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt); Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt);
// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output // inv_sqrt_controller calculates the inverse of the sqrt controller.
// This function calculates the input (aka error) to the sqrt_controller required to achieve a given output.
float inv_sqrt_controller(float output, float p, float D_max); float inv_sqrt_controller(float output, float p, float D_max);
// calculate the stopping distance for the square root controller based deceleration path // stopping_distance calculates the stopping distance for the square root controller based deceleration path.
float stopping_distance(float velocity, float p, float accel_max); float stopping_distance(float velocity, float p, float accel_max);
// calculate the maximum acceleration or velocity in a given direction // kinematic_limit calculates the maximum acceleration or velocity in a given direction.
// based on horizontal and vertical limits. // based on horizontal and vertical limits.
float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg); float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg);
// calculate the expo function on the normalised input // input_expo calculates the expo function on the normalised input.
// input must be in the range of -1 to 1 // The input must be in the range of -1 to 1.
// expo should be less than 1.0 but limited to be less than 0.95 // The expo should be less than 1.0 but limited to be less than 0.95.
float input_expo(float input, float expo); float input_expo(float input, float expo);