From 49674abb21f1c70361f81f95f647805bc75aa392 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 Jul 2016 11:47:44 +0900 Subject: [PATCH] Copter: guided mode's velocity controller stops before fence --- ArduCopter/Copter.h | 2 +- ArduCopter/control_guided.cpp | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6ea6e2953a..c3292f4a70 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -812,7 +812,7 @@ private: void guided_vel_control_run(); void guided_posvel_control_run(); void guided_angle_control_run(); - void guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_des); + void guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des); void guided_limit_clear(); void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); void guided_limit_init_time_and_pos(); diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index be4286e71c..ba556f0451 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -439,9 +439,9 @@ void Copter::guided_vel_control_run() // set velocity to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) { - guided_set_desired_velocity_with_accel_limits(Vector3f(0.0f,0.0f,0.0f)); + guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f)); } else { - guided_set_desired_velocity_with_accel_limits(guided_vel_target_cms); + guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); } // call velocity controller which includes z axis controller @@ -588,7 +588,7 @@ void Copter::guided_angle_control_run() } // helper function to update position controller's desired velocity while respecting acceleration limits -void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_des) +void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) { // get current desired velocity Vector3f curr_vel_des = pos_control.get_desired_velocity(); @@ -608,15 +608,16 @@ void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_d if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) { ratio_xy = vel_delta_xy_max / vel_delta_xy; } + curr_vel_des.x += (vel_delta.x * ratio_xy); + curr_vel_des.y += (vel_delta.y * ratio_xy); // limit z change float vel_delta_z_max = G_Dt * pos_control.get_accel_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 += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max); + // limit the velocity to prevent fence violations + avoid.adjust_velocity(pos_control.get_pos_xy_kP(), pos_control.get_accel_xy(), curr_vel_des); + // update position controller with new target pos_control.set_desired_velocity(curr_vel_des); }