diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 074dd10c7b..8582653c0c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -268,7 +268,7 @@ void AC_AttitudeControl_Heli::rate_controller_run() // Update Alt_Hold angle maximum void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in) { - float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_HELI_ANGLE_LIMIT_THROTTLE_MAX, 0.0f, 1.0f)); + float althold_lean_angle_max = acosf(constrain_float(_throttle_in/AC_ATTITUDE_HELI_ANGLE_LIMIT_THROTTLE_MAX, 0.0f, 1.0f)); _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 0601800d4d..0d52172470 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -188,7 +188,7 @@ void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in) return; } - float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f)); + float althold_lean_angle_max = acosf(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f)); _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index ac1b52a185..020702d324 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -195,7 +195,7 @@ void AC_AttitudeControl_Sub::update_althold_lean_angle_max(float throttle_in) return; } - float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f)); + float althold_lean_angle_max = acosf(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f)); _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max); }