From e85b1ac7402a364c3d814619af0bdf7eeb913bce Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 13 Jan 2018 15:20:36 +0900 Subject: [PATCH] Copter: pass dt to avoidance calls --- ArduCopter/Attitude.cpp | 2 +- ArduCopter/mode_guided.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 564d3aa66f..6934396773 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -283,7 +283,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current float Copter::get_avoidance_adjusted_climbrate(float target_rate) { #if AC_AVOID_ENABLED == ENABLED - avoid.adjust_velocity_z(pos_control->get_pos_z_kP(), pos_control->get_accel_z(), target_rate); + avoid.adjust_velocity_z(pos_control->get_pos_z_kP(), pos_control->get_accel_z(), target_rate, G_Dt); return target_rate; #else return target_rate; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 8e6ebf3398..b42083235b 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -653,7 +653,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const #if AC_AVOID_ENABLED // limit the velocity to prevent fence violations - _copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des); + _copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt); // get avoidance adjusted climb rate curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); #endif