ArduCopter: get_throttle_althold - add check to avoid divide by zero

ArduCopter/Attitude.pde

@@ -995,14 +995,18 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
     // calculate altitude error
     altitude_error    = target_alt - current_loc.alt;
 
-    linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
-
-    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));
+    // check kP to avoid division by zero
+    if( g.pi_alt_hold.kP() != 0 ) {
+        linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
+        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);
+        }
     }else{
-        desired_rate = g.pi_alt_hold.get_p(altitude_error);
+        desired_rate = 0;
     }
 
     desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);