Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: keep track of velocity liminting status

Browse Source
This commit is contained in:
Peter Hall 2019-08-22 18:12:45 +01:00 committed by Randy Mackay
parent ba811052f8
commit 556d90ca0c
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -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();
}
}
}

8
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -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;
};