diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a40cf8a7dd..f151e0a8e4 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1082,7 +1082,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up(); } else { // descend at up to WPNAV_SPEED_DN - climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down()); + climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -copter.wp_nav->get_default_speed_down(); } } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index d6dce7b534..3da5852da3 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -820,7 +820,7 @@ void ModeGuided::angle_control_run() float climb_rate_cms = 0.0f; if (!guided_angle_state.use_thrust) { // constrain climb rate - climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up()); + climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); // get avoidance adjusted climb rate climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);