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: proximity sensor method re-uses polygon fence boundary code

Browse Source
This commit is contained in:
Randy Mackay 2016-11-08 13:57:31 +09:00
parent cdee68174d
commit 55d79d9a32
2 changed files with 40 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -108,11 +108,46 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// get polygon boundary
// Note: first point in list is the return-point (which copter does not use)
uint16_t num_points;
Vector2f* boundary = _fence.get_polygon_points(num_points);
// adjust velocity using polygon
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points);
}
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel)
{
// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// get boundary from proximity sensor
uint16_t num_points;
const Vector2f *boundary = _proximity.get_boundary_points(num_points);
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points);
}
/*
* Adjusts the desired velocity for the polygon fence.
*/
void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points)
{
// exit if there are no points
if (boundary == nullptr || num_points == 0) {
return;
@ -154,37 +189,6 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2
desired_vel = safe_vel;
}
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel)
{
// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// normalise desired velocity vector
Vector2f vel_dir = desired_vel.normalized();
// get angle of desired velocity
float heading_rad = atan2f(vel_dir.y, vel_dir.x);
// rotate desired velocity angle into body-frame angle
float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw);
// get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame)
float distance_m;
if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) {
limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f));
}
}
/*
* Limits the component of desired_vel in the direction of the unit vector
* limit_direction to be at most the maximum speed permitted by the limit_distance.

5
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -54,6 +54,11 @@ private:
*/
void adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel);
/*
* Adjusts the desired velocity given an array of boundary points
*/
void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points);
/*
* Limits the component of desired_vel in the direction of the unit vector
* limit_direction to be at most the maximum speed permitted by the limit_distance.