From 90e205c90578ed56660811c814e95861c1eedde0 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 1 Apr 2014 20:40:08 +0900
Subject: [PATCH] AC_PosControl: replace safe_sqrt with pythagorous2

---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index ca21a54f8d..ee6400ecb6 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -394,7 +394,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
     float kP = _p_pos_xy.kP();
 
     // calculate current velocity
-    float vel_total = safe_sqrt(curr_vel.x*curr_vel.x + curr_vel.y*curr_vel.y);
+    float vel_total = pythagorous2(curr_vel.x, curr_vel.y);
 
     // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
     if (vel_total < 10.0f || kP <= 0.0f || _accel_cms <= 0.0f) {
@@ -502,7 +502,7 @@ void AC_PosControl::desired_vel_to_pos(float nav_dt)
     }
 
     // constrain and scale the desired velocity
-    vel_desired_total = safe_sqrt(_vel_desired.x*_vel_desired.x + _vel_desired.y*_vel_desired.y);
+    vel_desired_total = pythagorous2(_vel_desired.x, _vel_desired.y);
     if (vel_desired_total > _speed_cms && vel_desired_total > 0.0f) {
         _vel_desired.x = _speed_cms * _vel_desired.x/vel_desired_total;
         _vel_desired.y = _speed_cms * _vel_desired.y/vel_desired_total;
@@ -534,7 +534,7 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
         _pos_error.y = _pos_target.y - curr_pos.y;
 
         // constrain target position to within reasonable distance of current location
-        _distance_to_target = safe_sqrt(_pos_error.x*_pos_error.x + _pos_error.y*_pos_error.y);
+        _distance_to_target = pythagorous2(_pos_error.x, _pos_error.y);
         if (_distance_to_target > _leash && _distance_to_target > 0.0f) {
             _pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;
             _pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;
@@ -569,7 +569,7 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
         }
 
         // scale velocity to stays within limits
-        float vel_total = safe_sqrt(_vel_target.x*_vel_target.x + _vel_target.y*_vel_target.y);
+        float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
         if (vel_total > vel_max_from_pos_error) {
             _vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total;
             _vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total;
@@ -618,7 +618,7 @@ void AC_PosControl::rate_to_accel_xy(float dt)
 
     // scale desired acceleration if it's beyond acceptable limit
     // To-Do: move this check down to the accel_to_lean_angle method?
-    accel_total = safe_sqrt(_accel_target.x*_accel_target.x + _accel_target.y*_accel_target.y);
+    accel_total = pythagorous2(_accel_target.x, _accel_target.y);
     if (accel_total > POSCONTROL_ACCEL_XY_MAX) {
         _accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total;
         _accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total;