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

AP_Math: Target velocity can reduce when limited 

AP_Math: Target velocity can reduce when limited
This commit is contained in:
Leonard Hall 2022-12-20 00:44:53 +10:30 committed by Randy Mackay
parent 05aa879b61
commit ac3a3d9576
1 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Math/control.cpp
View File

@ -33,11 +33,17 @@
// vel_error - specifies the direction of the velocity error used in limit handling. // vel_error - specifies the direction of the velocity error used 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; float delta_vel = accel * dt;
// do not add delta_vel if it will increase the velocity error in the direction of limit // 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))) { // unless adding delta_vel will reduce vel towards zero
vel += delta_vel; if (is_positive(delta_vel * limit) && is_positive(vel_error * limit)) {
if (is_negative(vel * limit)) {
delta_vel = constrain_float(delta_vel, -fabsf(vel), fabsf(vel));
} else {
delta_vel = 0.0;
}
} }
vel += delta_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.
@ -49,9 +55,10 @@ 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. // 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); 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 // 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; delta_pos = 0.0;
} }
pos += delta_pos;
update_vel_accel(vel, accel, dt, limit, vel_error); update_vel_accel(vel, accel, dt, limit, vel_error);
} }
@ -63,13 +70,12 @@ void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, flo
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.
// unless adding delta_vel will reduce the magnitude of vel
Vector2f delta_vel = accel * dt; Vector2f delta_vel = accel * dt;
if (!limit.is_zero() && !delta_vel.is_zero()) { if (!limit.is_zero() && !delta_vel.is_zero()) {
// check if delta_vel will increase the velocity error in the direction of limit // check if delta_vel 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) && !is_negative(vel * limit)) {
// remove component of delta_vel in direction of limit delta_vel.zero();
Vector2f limit_unit = limit.normalized();
delta_vel -= limit_unit * (limit_unit * delta_vel);
} }
} }
vel += delta_vel; vel += delta_vel;