From 576af6d487c92aaf77b0ff7f943b74fcf250a4d4 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 23 Dec 2012 11:17:26 +0900 Subject: [PATCH] ArduCopter: bug fix to althold desired rate calculation. Fix from Jonathan. --- ArduCopter/Attitude.pde | 8 +++++--- ArduCopter/navigation.pde | 1 + 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ad0f1d7415..88c4b40c93 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 dc27d74861..981e368e9d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -612,6 +612,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); }