Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-07 16:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Math: Control: Support Accel only input

Browse Source
This commit is contained in:
Leonard Hall 2021-06-20 15:32:52 +09:30 committed by Randy Mackay
parent b3acdd49d6
commit de36398ebf
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Math/control.cpp
View File

@ -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. /* 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 function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.

3
libraries/AP_Math/control.h
View File

@ -53,6 +53,9 @@ void shape_accel(const float accel_input, float& accel,
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);
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. /* 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 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 : The kinematic path is constrained by :