From c8079a318c68ad2486661274ae381ae4dbe8c45b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Jun 2021 11:19:06 +1000 Subject: [PATCH] AC_PID: convert AC_P_2D to double --- libraries/AC_PID/AC_P_2D.cpp | 4 ++-- libraries/AC_PID/AC_P_2D.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp index b09f7f2f3b..a37e2ee9c1 100644 --- a/libraries/AC_PID/AC_P_2D.cpp +++ b/libraries/AC_PID/AC_P_2D.cpp @@ -24,12 +24,12 @@ AC_P_2D::AC_P_2D(float initial_p, float dt) : // update_all - set target and measured inputs to P controller and calculate outputs // limit is set true if the target has been moved to limit the maximum position error -Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit) +Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit) { limit = false; // calculate distance _error - _error = Vector2f{target_x, target_y} - measurement; + _error = (Vector2p{target_x, target_y} - measurement.topostype()).tofloat(); // Constrain _error and target position // Constrain the maximum length of _vel_target to the maximum position correction velocity diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h index ecef569566..d547a07da1 100644 --- a/libraries/AC_PID/AC_P_2D.h +++ b/libraries/AC_PID/AC_P_2D.h @@ -20,11 +20,11 @@ public: void set_dt(float dt) { _dt = dt; } // set target and measured inputs to P controller and calculate outputs - Vector2f update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit) WARN_IF_UNUSED; + Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit) WARN_IF_UNUSED; // set target and measured inputs to P controller and calculate outputs // measurement is provided as 3-axis vector but only x and y are used - Vector2f update_all(float &target_x, float &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED { + Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED { return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}, limit); }