From 556d90ca0c9e71db4043825af1b0b70f5c781780 Mon Sep 17 00:00:00 2001 From: Peter Hall <33176108+IamPete1@users.noreply.github.com> Date: Thu, 22 Aug 2019 18:12:45 +0100 Subject: [PATCH] AC_Avoidance: keep track of velocity liminting status --- libraries/AC_Avoidance/AC_Avoid.cpp | 7 ++++++- libraries/AC_Avoidance/AC_Avoid.h | 8 +++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 4fe87078fe..e7a8a396a9 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -203,6 +203,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // limit climb rate const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt); climb_rate_cms = MIN(max_speed, climb_rate_cms); + _last_limit_time = AP_HAL::millis(); } } @@ -261,7 +262,7 @@ void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max) * Uses velocity adjustment idea from Randy's second email on this thread: * https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ */ -void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) const +void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) { const float max_speed = get_max_speed(kP, accel_cmss, limit_distance_cm, dt); // project onto limit direction @@ -269,6 +270,7 @@ void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_ if (speed > max_speed) { // subtract difference between desired speed and maximum acceptable speed desired_vel_cms += limit_direction*(max_speed - speed); + _last_limit_time = AP_HAL::millis(); } } @@ -354,6 +356,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f if (is_positive(distance_to_target)) { const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt); desired_vel_cms = target_direction * (MIN(desired_speed,max_speed) / distance_to_target); + _last_limit_time = AP_HAL::millis(); } } else { // implement stopping behaviour @@ -366,6 +369,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f // otherwise user is backing away from fence so do not apply limits if (stopping_point_plus_margin_dist_from_home >= dist_from_home) { desired_vel_cms.zero(); + _last_limit_time = AP_HAL::millis(); } } else { // shorten vector without adjusting its direction @@ -375,6 +379,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt); if (max_speed < desired_speed) { desired_vel_cms *= MAX(max_speed, 0.0f) / desired_speed; + _last_limit_time = AP_HAL::millis(); } } } diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index dbf554b22d..bc95efa6f6 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -18,6 +18,8 @@ #define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 5.0f // objects over 5m away are ignored (default value for DIST_MAX parameter) #define AC_AVOID_ANGLE_MAX_PERCENT 0.75f // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle +#define AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS 500 // if limiting is active if last limit is happend in the last x ms + /* * This class prevents the vehicle from leaving a polygon fence in * 2 dimensions by limiting velocity (adjust_velocity). @@ -72,7 +74,7 @@ public: // limit_direction to be at most the maximum speed permitted by the limit_distance_cm. // uses velocity adjustment idea from Randy's second email on this thread: // https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ - void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) const; + void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt); // compute the speed such that the stopping distance of the vehicle will // be exactly the input distance. @@ -82,6 +84,9 @@ public: // return margin (in meters) that the vehicle should stay from objects float get_margin() const { return _margin; } + // return true if limiting is active + bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;}; + static const struct AP_Param::GroupInfo var_info[]; private: @@ -148,6 +153,7 @@ private: AP_Int8 _behavior; // avoidance behaviour (slide or stop) bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) + uint32_t _last_limit_time; // the last time a limit was active static AC_Avoid *_singleton; };