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

AP_Math: update_pos_vel_accel methods accept limit as const reference 

also update some comments
This commit is contained in:
Randy Mackay 2021-11-23 19:58:44 +09:00
parent 109d543b80
commit 092730c32e
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Math/control.cpp
View File

@ -37,7 +37,7 @@ void update_vel_accel(float& vel, float accel, float dt, float limit)
} }
} }
// update_pos_vel_accel - single 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 - 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
void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit) void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit)
{ {
@ -52,7 +52,7 @@ 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
void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit) void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit)
{ {
// 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.
Vector2f delta_vel = accel * dt; Vector2f delta_vel = accel * dt;
@ -68,7 +68,7 @@ void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, Vector2
// 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
void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit) void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit)
{ {
// move position and velocity forward by dt. // move position and velocity forward by dt.
Vector2f delta_pos = vel * dt + accel * 0.5f * sq(dt); Vector2f delta_pos = vel * dt + accel * 0.5f * sq(dt);

8
libraries/AP_Math/control.h
View File

@ -29,15 +29,15 @@ typedef Vector3f Vector3p;
// update_vel_accel - single axis projection. // update_vel_accel - single axis projection.
void update_vel_accel(float& vel, float accel, float dt, float limit); void update_vel_accel(float& vel, float accel, float dt, float limit);
// update_vel_accel projects the velocity, vel, 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.
// update_vel_accel - single axis projection. // the position and velocity is not moved in the direction of limit if limit is not set to zero
void update_pos_vel_accel(postype_t & pos, float& vel, float accel, float dt, float limit); void update_pos_vel_accel(postype_t & pos, float& vel, float accel, float dt, float limit);
// update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs. // update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit); void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit);
// update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs. // update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit); void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit);
/* 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.