Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: cleanups from Leonards feedback

Browse Source
This commit is contained in:
Andrew Tridgell 2021-06-23 09:49:24 +10:00
parent 6bde607aaa
commit 889bd2547f
2 changed files with 20 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AP_Math/control.cpp
View File

@ -94,7 +94,7 @@ void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel
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 input accel to be jerk limited The function alters the variable accel to follow a jerk limited kinematic path to accel_input
*/ */
void shape_accel(const float accel_input, float& accel, void shape_accel(const float accel_input, float& accel,
const float accel_min, const float accel_max, const float accel_min, const float accel_max,
@ -126,21 +126,9 @@ void shape_accel(const float accel_input, float& accel,
} }
} }
/* shape_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity. // 2D version
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:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
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.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The accel_max limit can be removed by setting it to zero.
*/
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
float accel_max, float tc, float dt) float accel_max, float tc, float dt)
{ {
// sanity check tc // sanity check tc
if (!is_positive(tc)) { if (!is_positive(tc)) {
@ -177,11 +165,10 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
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 input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input
to be jerk limited
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, const float accel_input, void shape_vel_accel(const float vel_input1, const float accel_input,
const float vel, float& accel, const float vel, float& accel,
const float vel_min, const float vel_max, const float vel_min, const float vel_max,
const float accel_min, const float accel_max, const float accel_min, const float accel_max,
@ -195,6 +182,9 @@ void shape_vel_accel(float &vel_input, const float accel_input,
// Calculate time constants and limits to ensure stable operation // Calculate time constants and limits to ensure stable operation
const float KPa = 1.0 / tc; const float KPa = 1.0 / tc;
// we are changing vel_input, but don't want the change in the caller
float vel_input = vel_input1;
// limit velocity to vel_max // limit velocity to vel_max
if (is_negative(vel_min) && is_positive(vel_max)){ if (is_negative(vel_min) && is_positive(vel_max)){
vel_input = constrain_float(vel_input, vel_min, vel_max); vel_input = constrain_float(vel_input, vel_min, vel_max);
@ -212,20 +202,8 @@ void shape_vel_accel(float &vel_input, const float accel_input,
shape_accel(accel_target, accel, accel_min, accel_max, tc, dt); shape_accel(accel_target, accel, accel_min, accel_max, tc, dt);
} }
/* shape_vel_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity. // 2D version
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. void shape_vel_accel_xy(const Vector2f &vel_input1, const Vector2f& accel_input,
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
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.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The accel_max limit can be removed by setting it to zero.
*/
void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
const Vector2f& vel, Vector2f& accel, const Vector2f& vel, Vector2f& accel,
const float vel_max, const float accel_max, const float tc, const float dt) const float vel_max, const float accel_max, const float tc, const float dt)
{ {
@ -233,6 +211,7 @@ void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
if (!is_positive(tc)) { if (!is_positive(tc)) {
return; return;
} }
Vector2f vel_input = vel_input1;
// limit velocity to vel_max // limit velocity to vel_max
if (is_negative(vel_max)) { if (is_negative(vel_max)) {
@ -263,10 +242,10 @@ void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
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 input position to be the closest position that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited 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(float &pos_input, const float vel_input, const float accel_input, void shape_pos_vel_accel(const float pos_input, const float vel_input, const float accel_input,
const float pos, const float vel, float& accel, const float pos, const float vel, float& accel,
const float vel_correction_max, const float vel_min, const float vel_max, const float vel_correction_max, const float vel_min, const float vel_max,
const float accel_min, const float accel_max, const float tc, const float dt) const float accel_min, const float accel_max, const float tc, const float dt)
@ -297,18 +276,7 @@ void shape_pos_vel_accel(float &pos_input, const float vel_input, const float ac
shape_vel_accel(vel_target, accel_input, vel, accel, vel_min, vel_max, accel_min, accel_max, tc, dt); shape_vel_accel(vel_target, accel_input, vel, accel, vel_min, vel_max, accel_min, accel_max, tc, dt);
} }
/* shape_pos_vel_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity. // 2D version
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:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
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.
The function alters the accel to be jerk limited
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_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2f& pos, const Vector2f& vel, Vector2f& accel, const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
const float vel_correction_max, const float vel_max, const float accel_max, const float tc, const float dt) const float vel_correction_max, const float vel_max, const float accel_max, const float tc, const float dt)

41
libraries/AP_Math/control.h
View File

@ -26,12 +26,11 @@ void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel
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 input accel to be jerk limited The function alters the variable accel to follow a jerk limited kinematic path to accel_input
*/ */
void shape_accel(const float accel_input, float& accel, void shape_accel(const float accel_input, float& accel,
const float accel_min, const float accel_max, const float accel_min, const float accel_max,
const float tc, const float dt); const float tc, const float dt);
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
const float accel_max, const float tc, const float dt); const float accel_max, const float tc, const float dt);
@ -44,29 +43,14 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
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 input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel The function alters the variable accel to follow a jerk limited kinematic path to vel_input and accel_input
to be jerk limited
*/ */
void shape_vel_accel(float &vel_input, const float accel_input, void shape_vel_accel(const float vel_input, const float accel_input,
const float vel, float& accel, const float vel, float& accel,
const float vel_min, const float vel_max, const float vel_min, const float vel_max,
const float accel_min, const float accel_max, const float accel_min, const float accel_max,
const float tc, const float dt); const float tc, const float dt);
void shape_vel_accel_xy(const Vector2f &vel_input, const Vector2f& accel_input,
/* 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:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
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.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The accel_max limit can be removed by setting it to zero.
*/
void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
const Vector2f& vel, Vector2f& accel, const Vector2f& vel, Vector2f& accel,
const float vel_max, const float accel_max, const float tc, const float dt); const float vel_max, const float accel_max, const float tc, const float dt);
@ -79,25 +63,12 @@ void shape_vel_accel_xy(Vector2f &vel_input, const Vector2f& accel_input,
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 input position to be the closest position that the system could reach zero acceleration in the minimum time and alters the accel to be jerk limited The function alters the variable accel to follow a jerk limited kinematic path to pos_input, vel_input and accel_input
*/ */
void shape_pos_vel_accel(float &pos_input, const float vel_input, const float accel_input, void shape_pos_vel_accel(const float pos_input, const float vel_input, const float accel_input,
const float pos, const float vel, float& accel, const float pos, const float vel, float& accel,
const float vel_correction_max, const float vel_min, const float vel_max, const float vel_correction_max, const float vel_min, const float vel_max,
const float accel_min, const float accel_max, const float tc, const float dt); const float accel_min, const float accel_max, const float tc, const float dt);
/* shape_pos_vel_xy 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:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
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.
The function alters the accel to be jerk limited
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_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2f& pos, const Vector2f& vel, Vector2f& accel, const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
const float vel_correction_max, const float vel_max, const float accel_max, const float tc, const float dt); const float vel_correction_max, const float vel_max, const float accel_max, const float tc, const float dt);