From de36398ebf54d3b8c3ed4771b6f26e87fdd163e1 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 20 Jun 2021 15:32:52 +0930 Subject: [PATCH] AC_Math: Control: Support Accel only input --- libraries/AP_Math/control.cpp | 11 +++++++++++ libraries/AP_Math/control.h | 3 +++ 2 files changed, 14 insertions(+) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 05e60e1952..d53d7d8261 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -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. diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index 2905807053..b6c5f9c5d7 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -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 :