From daac540e089cf566fa33e4da206584cc0ed7eb66 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 26 Dec 2012 21:18:16 +0900 Subject: [PATCH] ArduCopter: get_throttle_althold - add check to avoid divide by zero --- ArduCopter/Attitude.pde | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 88c4b40c93..7c2f48f153 100644 --- a/ArduCopter/Attitude.pde +++ b/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);