mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 07:58:28 -04:00
AC_Math: Control: Support Accel only input
This commit is contained in:
parent
b3acdd49d6
commit
de36398ebf
libraries/AP_Math
@ -155,6 +155,17 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
|
||||
}
|
||||
}
|
||||
|
||||
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
|
||||
float accel_max, float tc, float dt)
|
||||
{
|
||||
const Vector2f accel_input_2f {accel_input.x, accel_input.y};
|
||||
Vector2f accel_2f {accel.x, accel.y};
|
||||
|
||||
shape_accel_xy(accel_input_2f, accel_2f, accel_max, tc, dt);
|
||||
accel.x = accel_2f.x;
|
||||
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.
|
||||
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
|
||||
|
@ -53,6 +53,9 @@ void shape_accel(const float accel_input, float& accel,
|
||||
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
|
||||
const float accel_max, const float tc, const float dt);
|
||||
|
||||
void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel,
|
||||
float accel_max, float tc, float dt);
|
||||
|
||||
/* shape_vel calculates a jerk limited path from the current 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 kinematic path is constrained by :
|
||||
|
Loading…
Reference in New Issue
Block a user