Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Copter: fix take-off velocity limits #21361

Merged
merged 1 commit into from Aug 9, 2022
Merged

Copter: fix take-off velocity limits #21361

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
104 changes: 54 additions & 50 deletions libraries/AP_Math/control.cpp
View file
Open in desktop
Expand Up @@ -35,7 +35,7 @@ void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_
{
const float delta_vel = accel * dt;
// do not add delta_vel if it will increase the velocity error in the direction of limit
if (!(is_positive(delta_vel * limit) && is_positive(vel_error * limit))){
if (!(is_positive(delta_vel * limit) && is_positive(vel_error * limit))) {
vel += delta_vel;
}
}
Expand All @@ -49,7 +49,7 @@ void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, flo
// move position and velocity forward by dt if it does not increase error when limited.
float delta_pos = vel * dt + accel * 0.5f * sq(dt);
// do not add delta_pos if it will increase the velocity error in the direction of limit
if (!(is_positive(delta_pos * limit) && is_positive(pos_error * limit))){
if (!(is_positive(delta_pos * limit) && is_positive(pos_error * limit))) {
pos += delta_pos;
}

Expand Down Expand Up @@ -99,33 +99,37 @@ 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.
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 :
acceleration limits - accel_min, 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.
maximum jerk - jerk_max (must be positive).
The function alters the variable accel to follow a jerk limited kinematic path to accel_input.
*/
void shape_accel(float accel_input, float& accel,
float jerk_max, float dt)
{
// sanity check jerk_max
if (!is_positive(jerk_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if we should just not apply the jerk limit in this case. Early return means no more Z control?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There are a bunch of places that this is true. But it should be already checked earlier in the code so it should never be triggered.

}

// jerk limit acceleration change
float accel_delta = accel_input - accel;
if (is_positive(jerk_max)) {
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
}
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
accel += accel_delta;
}

// 2D version
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
float jerk_max, float dt)
{
// sanity check jerk_max
if (!is_positive(jerk_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}

// jerk limit acceleration change
Vector2f accel_delta = accel_input - accel;
if (is_positive(jerk_max)) {
accel_delta.limit_length(jerk_max * dt);
}
accel_delta.limit_length(jerk_max * dt);
accel = accel + accel_delta;
}

Expand All @@ -143,22 +147,19 @@ void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
/* 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 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.
minimum acceleration - accel_min (must be negative),
maximum acceleration - accel_max (must be positive),
maximum jerk - jerk_max (must be positive).
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 correction acceleration is limited from accel_min to accel_max. If limit_total is true the target acceleration is limited from accel_min to accel_max.
*/
void shape_vel_accel(float vel_input, float accel_input,
float vel, float& accel,
float accel_min, float accel_max,
float jerk_max, float dt, bool limit_total_accel)
{
// sanity check accel_max
if (!(is_negative(accel_min) && is_positive(accel_max))) {
// sanity check accel_min, accel_max and jerk_max.
if (!is_negative(accel_min) || !is_positive(accel_max) || !is_positive(jerk_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}
Expand Down Expand Up @@ -199,8 +200,8 @@ void shape_vel_accel_xy(const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2f& vel, Vector2f& accel,
float accel_max, float jerk_max, float dt, bool limit_total_accel)
{
// sanity check accel_max
if (!is_positive(accel_max)) {
// sanity check accel_max and jerk_max.
if (!is_positive(accel_max) || !is_positive(jerk_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}
Expand All @@ -223,7 +224,7 @@ void shape_vel_accel_xy(const Vector2f& vel_input, const Vector2f& accel_input,
float accel_dir = vel_input_unit * accel_target;
Vector2f accel_cross = accel_target - (vel_input_unit * accel_dir);

// ensure 1/sqrt(2) of maximum acceleration is availible to correct cross component
// ensure 1/sqrt(2) of maximum acceleration is available to correct cross component
// relative to vel_input
if (sq(accel_dir) <= accel_cross.length_squared()) {
// accel_target can be simply limited in magnitude
Expand Down Expand Up @@ -252,28 +253,27 @@ 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.
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.
minimum velocity - vel_min (must not be positive),
maximum velocity - vel_max (must not be negative),
minimum acceleration - accel_min (must be negative),
maximum acceleration - accel_max (must be positive),
maximum jerk - jerk_max (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 vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
The correction velocity is limited to vel_max to vel_min. If limit_total is true the target velocity is limited to vel_max to vel_min.
The correction acceleration is limited from accel_min to accel_max. If limit_total is true the target acceleration is limited from accel_min to accel_max.
*/
void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input,
postype_t pos, float vel, float& accel,
float vel_min, float vel_max,
float accel_min, float accel_max,
float jerk_max, float dt, bool limit_total_accel)
float jerk_max, float dt, bool limit_total)
{
// sanity check accel_max
if (!(is_negative(accel_min) && is_positive(accel_max))) {
// sanity check vel_min, vel_max, accel_min, accel_max and jerk_max.
if (is_positive(vel_min) || is_negative(vel_max) || !is_negative(accel_min) || !is_positive(accel_max) || !is_positive(jerk_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}


// position error to be corrected
float pos_error = pos_input - pos;

Expand All @@ -294,25 +294,28 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
// velocity to correct position
float vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);

// limit velocity to vel_max
if (is_negative(vel_min) && is_positive(vel_max)){
vel_target = constrain_float(vel_target, vel_min, vel_max);
}
// limit velocity between vel_min and vel_max
vel_target = constrain_float(vel_target, vel_min, vel_max);

// velocity correction with input velocity
vel_target += vel_input;

shape_vel_accel(vel_target, accel_input, vel, accel, accel_min, accel_max, jerk_max, dt, limit_total_accel);
// limit velocity between vel_min and vel_max
if (limit_total) {
vel_target = constrain_float(vel_target, vel_min, vel_max);
}

shape_vel_accel(vel_target, accel_input, vel, accel, accel_min, accel_max, jerk_max, dt, limit_total);
}

// 2D version
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,
float vel_max, float accel_max,
float jerk_max, float dt, bool limit_total_accel)
float jerk_max, float dt, bool limit_total)
{
// sanity check accel_max
if (!is_positive(accel_max)) {
// sanity check vel_max, accel_max and jerk_max.
if (is_negative(vel_max) || !is_positive(accel_max) || !is_positive(jerk_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}
Expand All @@ -329,16 +332,17 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);

// limit velocity to vel_max
if (is_negative(vel_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
} else if (is_positive(vel_max)) {
vel_target.limit_length(vel_max);
}
vel_target.limit_length(vel_max);

// velocity correction with input velocity
vel_target = vel_target + vel_input;

// limit velocity to vel_max
if (limit_total) {
vel_target.limit_length(vel_max);
}

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

/* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
Expand Down
36 changes: 15 additions & 21 deletions libraries/AP_Math/control.h
View file
Open in desktop
Expand Up @@ -51,11 +51,7 @@ 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.
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 :
acceleration limits - accel_min, 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.
maximum jerk - jerk_max (must be positive).
The function alters the variable accel to follow a jerk limited kinematic path to accel_input.
*/
void shape_accel(float accel_input, float& accel,
Expand All @@ -71,14 +67,11 @@ void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
/* 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 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.
minimum acceleration - accel_min (must be negative),
maximum acceleration - accel_max (must be positive),
maximum jerk - jerk_max (must be positive).
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 correction acceleration is limited from accel_min to accel_max. If limit_total is true the target acceleration is limited from accel_min to accel_max.
*/
void shape_vel_accel(float vel_input, float accel_input,
float vel, float& accel,
Expand All @@ -93,26 +86,27 @@ void shape_vel_accel_xy(const Vector2f& vel_input1, 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.
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.
minimum velocity - vel_min (must be negative),
maximum velocity - vel_max (must be positive),
minimum acceleration - accel_min (must be negative),
maximum acceleration - accel_max (must be positive),
maximum jerk - jerk_max (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 vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
The correction velocity is limited to vel_max to vel_min. If limit_total is true the target velocity is limited to vel_max to vel_min.
The correction acceleration is limited from accel_min to accel_max. If limit_total is true the target acceleration is limited from accel_min to accel_max.
*/
void shape_pos_vel_accel(const postype_t pos_input, float vel_input, float accel_input,
const postype_t pos, float vel, float& accel,
float vel_min, float vel_max,
float accel_min, float accel_max,
float jerk_max, float dt, bool limit_total_accel);
float jerk_max, float dt, bool limit_total);

// 2D version
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,
float vel_max, float accel_max,
float jerk_max, float dt, bool limit_total_accel);
float jerk_max, float dt, bool limit_total);


/* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
Input parameters are:
Expand Down