Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: add and use AP_BEACON_ENABLED

This commit is contained in:
Peter Barker 2022-01-28 11:09:42 +11:00 committed by Peter Barker
parent 267353d280
commit 1e9d0f59c0
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -156,12 +156,14 @@ void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desir
}
#endif // AP_FENCE_ENABLED
#if AP_BEACON_ENABLED
if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) {
// Store velocity needed to back away from beacon fence
Vector2f backup_vel_beacon;
adjust_velocity_beacon_fence(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_beacon, dt);
find_max_quadrant_velocity(backup_vel_beacon, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel);
}
#endif // AP_BEACON_ENABLED
// check for vertical fence
float desired_velocity_z_cms = desired_vel_cms.z;
@ -1089,6 +1091,7 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec
}
#endif // AP_FENCE_ENABLED
#if AP_BEACON_ENABLED
/*
* Adjusts the desired velocity for the beacon fence.
*/
@ -1117,6 +1120,7 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
#endif
adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, margin, dt, true);
}
#endif // AP_BEACON_ENABLED
/*
* Adjusts the desired velocity based on output from the proximity sensor