diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 221c0666a44a12efbbc88df9754df94b6a3e82a9..2edaa6ed616df2088e4c10761e1f01a59c03f262 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -997,10 +997,12 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
 
     linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
 
-    if( altitude_error < linear_distance*2 ) {
-        desired_rate = g.pi_alt_hold.get_p(altitude_error);
-    }else{
+    if( altitude_error > 2*linear_distance ) {
         desired_rate = sqrt(2*250*(altitude_error-linear_distance));
+    }else if( altitude_error < -2*linear_distance ) {
+        desired_rate = -sqrt(2*250*(-altitude_error-linear_distance));
+    }else{
+        desired_rate = g.pi_alt_hold.get_p(altitude_error);
     }
 
     desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 90bf8c2f4be084abc2322a69ade300686bbf7ca1..decb98b0a6dd3dc81a9bc5bd965eaf954f299a5e 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -611,6 +611,7 @@ static int16_t get_desired_speed(int16_t max_speed)
     	 	int32_t temp 	= 2 * 100 * (int32_t)(wp_distance - g.waypoint_radius * 100);
     	 	int32_t s_min 	= WAYPOINT_SPEED_MIN;
     	 	temp 			+= s_min * s_min;
+            if( temp < 0 ) temp = 0;                // check to ensure we don't try to take the sqrt of a negative number
     		max_speed 		= sqrt((float)temp);
             max_speed 		= min(max_speed, g.waypoint_speed_max);
         }