From 7a6e0a981b84ebb8e91e108452a9afdd9630cf73 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 Jul 2016 11:40:08 +0900 Subject: [PATCH] Copter: simplify guided mode's velocity controller's accel limit calcs --- ArduCopter/control_guided.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 2c20614806..be4286e71c 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -610,17 +610,12 @@ void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_d } // limit z change - float vel_delta_z = fabsf(vel_delta.z); float vel_delta_z_max = G_Dt * pos_control.get_accel_z(); - float ratio_z = 1.0f; - if (!is_zero(vel_delta_z) && (vel_delta_z > vel_delta_z_max)) { - ratio_z = vel_delta_z_max / vel_delta_z; - } // new target curr_vel_des.x += (vel_delta.x * ratio_xy); curr_vel_des.y += (vel_delta.y * ratio_xy); - curr_vel_des.z += (vel_delta.z * ratio_z); + curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max); // update position controller with new target pos_control.set_desired_velocity(curr_vel_des);