From 838831d30ee4a62bb7e240c65e871e1120d842c7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Mar 2022 09:25:36 +1100 Subject: [PATCH] AC_WPNav: use angle/accel functions --- libraries/AC_WPNav/AC_Loiter.cpp | 2 +- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index edfd47ada6..0e66fdfe44 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -211,7 +211,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on) float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f); gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN); - float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f)); + float pilot_acceleration_max = angle_to_accel(get_angle_max_cd()*0.01) * 100; // range check nav_dt if (nav_dt < 0) { diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 032a013d45..fb30863f56 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -150,7 +150,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) // sanity check parameters // check _wp_accel_cmss is reasonable - _scurve_accel_corner = GRAVITY_MSS * 100.0f * tanf(ToRad(_pos_control.get_lean_angle_max_cd() * 0.01f)); + _scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100; const float wp_accel_cmss = MIN(_wp_accel_cmss, _scurve_accel_corner); _wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss);