From ae4ded86a8db7323f505a0e0dadbc2e44a75d0f1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 14 Dec 2017 10:42:17 +0900 Subject: [PATCH] AC_Avoid: get_max_speed supports linear acceleration Also get_stopping_distance supports linear deceleration --- libraries/AC_Avoidance/AC_Avoid.cpp | 24 ++++++++++++++++-------- libraries/AC_Avoidance/AC_Avoid.h | 5 +++-- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 4d6bb3aff0..7003613c98 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -477,10 +477,13 @@ void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, * Computes the speed such that the stopping distance * of the vehicle will be exactly the input distance. */ -float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance, float dt) const +float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const { - // should use time instead of 0.0f - return AC_AttitudeControl::sqrt_controller(distance, kP, accel_cmss, dt); + if (is_zero(kP)) { + return safe_sqrt(2.0f * distance_cm * accel_cmss); + } else { + return AC_AttitudeControl::sqrt_controller(distance_cm, kP, accel_cmss, dt); + } } /* @@ -488,20 +491,25 @@ float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance, float * * Implementation copied from AC_PosControl. */ -float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed) const +float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed_cms) const { // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero - if (kP <= 0.0f || accel_cmss <= 0.0f || is_zero(speed)) { + if (accel_cmss <= 0.0f || is_zero(speed_cms)) { return 0.0f; } + // handle linear deceleration + if (kP <= 0.0f) { + return 0.5f * sq(speed_cms) / accel_cmss; + } + // calculate distance within which we can stop // accel_cmss/kP is the point at which velocity switches from linear to sqrt - if (speed < accel_cmss/kP) { - return speed/kP; + if (speed_cms < accel_cmss/kP) { + return speed_cms/kP; } else { // accel_cmss/(2.0f*kP*kP) is the distance at which we switch from linear to sqrt response - return accel_cmss/(2.0f*kP*kP) + (speed*speed)/(2.0f*accel_cmss); + return accel_cmss/(2.0f*kP*kP) + (speed_cms*speed_cms)/(2.0f*accel_cmss); } } diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index e34cdfdd0f..00a23258eb 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -110,13 +110,14 @@ private: /* * Computes the speed such that the stopping distance * of the vehicle will be exactly the input distance. + * kP should be non-zero for Copter which has a non-linear response */ - float get_max_speed(float kP, float accel_cmss, float distance, float dt) const; + float get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const; /* * Computes distance required to stop, given current speed. */ - float get_stopping_distance(float kP, float accel_cmss, float speed) const; + float get_stopping_distance(float kP, float accel_cmss, float speed_cms) const; /* * methods for avoidance in non-GPS flight modes