From 9b91cfe4ee18eb96a3de4fb3b821bdb6e52521ea Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Jun 2021 11:18:41 +1000 Subject: [PATCH] AP_Math: implement double versions of some position control methods --- libraries/AP_Math/control.cpp | 16 ++++++++-------- libraries/AP_Math/control.h | 12 ++++++------ libraries/AP_Math/vector2.cpp | 12 ------------ libraries/AP_Math/vector2.h | 8 ++++++-- libraries/AP_Math/vector3.h | 8 ++++++-- 5 files changed, 26 insertions(+), 30 deletions(-) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 9e3cd85f04..05e60e1952 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -39,7 +39,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. // the position and velocity is not moved in the direction of limit if limit is not set to zero -void update_pos_vel_accel(float& 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) { // move position and velocity forward by dt if it does not increase error when limited. float delta_pos = vel * dt + accel * 0.5f * sq(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. // 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(Vector2f& 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, Vector2f limit) { // move position and velocity forward by dt. Vector2f delta_pos = vel * dt + accel * 0.5f * sq(dt); @@ -81,7 +81,7 @@ void update_pos_vel_accel_xy(Vector2f& pos, Vector2f& vel, const Vector2f& accel } } - pos += delta_pos; + pos += delta_pos.topostype(); update_vel_accel_xy(vel, accel, dt, limit); } @@ -245,8 +245,8 @@ void shape_vel_accel_xy(const Vector2f &vel_input1, const Vector2f& accel_input, 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. */ -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, +void shape_pos_vel_accel(const postype_t pos_input, const float vel_input, const float accel_input, + const postype_t pos, const float vel, float& accel, 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) { @@ -277,8 +277,8 @@ void shape_pos_vel_accel(const float pos_input, const float vel_input, const flo } // 2D version -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, +void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, + const Vector2p& 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) { if (!is_positive(tc)) { @@ -290,7 +290,7 @@ void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input const float accel_tc_max = accel_max*(1.0f - 1.0f/CONTROL_TIME_CONSTANT_RATIO); // position error to be corrected - Vector2f pos_error = pos_input - pos; + Vector2f pos_error = (pos_input - pos).tofloat(); // velocity to correct position Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt); diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index b4beda6769..2905807053 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -29,13 +29,13 @@ 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_vel_accel - single axis projection. -void update_pos_vel_accel(float& 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. void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit); // 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(Vector2f& 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, Vector2f limit); /* 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. @@ -84,12 +84,12 @@ void shape_vel_accel_xy(const Vector2f &vel_input, const Vector2f& accel_input, The time constant must be positive. 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(const float pos_input, const float vel_input, const float accel_input, - const float pos, const float vel, float& accel, +void shape_pos_vel_accel(const postype_t pos_input, const float vel_input, const float accel_input, + const postype_t pos, const float vel, float& accel, 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); -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, +void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input, const Vector2f& accel_input, + const Vector2p& 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); // proportional controller with piecewise sqrt sections to constrain second derivative diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp index 26f468b264..2251ba1eb9 100644 --- a/libraries/AP_Math/vector2.cpp +++ b/libraries/AP_Math/vector2.cpp @@ -448,18 +448,6 @@ void Vector2::rotate(float angle_rad) y = ry; } -template -Vector2 Vector2::todouble(void) const -{ - return Vector2d{x,y}; -} - -template -Vector2 Vector2::tofloat(void) const -{ - return Vector2f{float(x),float(y)}; -} - // define for float and double template class Vector2; template class Vector2; diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 903ded4e52..0b087268fd 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -165,8 +165,12 @@ struct Vector2 /* conversion to/from double */ - Vector2 tofloat() const; - Vector2 todouble() const; + Vector2 tofloat() const { + return Vector2{float(x),float(y)}; + } + Vector2 todouble() const { + return Vector2{x,y}; + } // given a position p1 and a velocity v1 produce a vector // perpendicular to v1 maximising distance from p1 diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 1479914b6d..c5ad55ea5f 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -268,8 +268,12 @@ public: /* conversion to/from double */ - Vector3 tofloat() const; - Vector3 todouble() const; + Vector3 tofloat() const { + return Vector3{float(x),float(y),float(z)}; + } + Vector3 todouble() const { + return Vector3{x,y,z}; + } // given a position p1 and a velocity v1 produce a vector // perpendicular to v1 maximising distance from p1. If p1 is the