From a1926441da82b9d2cc206d97f131b3745dc38aff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 30 May 2013 18:25:04 +0900 Subject: [PATCH] Copter: use fast tan for accel to lean angle calcs Committed on behalf of Leonard --- libraries/AC_WPNav/AC_WPNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 366763690f..bfef05fc35 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -585,8 +585,8 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw; // update angle targets that will be passed to stabilize controller - _desired_roll = constrain_float((accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); - _desired_pitch = constrain_float((-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); + _desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); + _desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); } // get_bearing_cd - return bearing in centi-degrees between two positions