From 84ac7213409bef81a2cf1038f7a9144df473b479 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 May 2015 13:44:19 +1000 Subject: [PATCH] AC_AttitudeControl: use M_PI_F instead of (float)M_PI --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index ac41b2e6a6..6cc7f93f95 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -876,9 +876,9 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler) accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller - _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/(float)M_PI),-lean_angle_max, lean_angle_max); - float cos_pitch_target = cosf(_pitch_target*(float)M_PI/18000); - _roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/(float)M_PI), -lean_angle_max, lean_angle_max); + _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI_F),-lean_angle_max, lean_angle_max); + float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000); + _roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI_F), -lean_angle_max, lean_angle_max); } // get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s