diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp
index 72dc5fb22c..e876840313 100644
--- a/libraries/AP_InertialNav/AP_InertialNav.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav.cpp
@@ -283,7 +283,7 @@ void AP_InertialNav::set_velocity_xy(float x, float y)
 // set_velocity_xy - set velocity in latitude & longitude directions (in cm/s)
 float AP_InertialNav::get_velocity_xy()
 {
-	return safe_sqrt(_velocity.x * _velocity.x + _velocity.y * _velocity.y);
+	return pythagorous2(_velocity.x, _velocity.y);
 }
 
 //