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

AC_Avoid: Let user take control if they are backing away at a greater speed than what we have calculated

Browse Source
This commit is contained in:
Rishabh 2020-12-27 22:01:05 +05:30 committed by Randy Mackay
parent 91132d00d9
commit d359c5ddf2
1 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -193,26 +193,29 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel
if (desired_backup_vel.length() > max_back_spd_cms) { if (desired_backup_vel.length() > max_back_spd_cms) {
desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms; desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms;
} }
if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { // let user take control if they are backing away at a greater speed than what we have calculated
// project desired velocity towards backup velocity horizontally // this has to be done for x,y,z seperately. For eg, user is doing fine in "x" direction but might need backing up in "y".
const Vector2f backup_vel_xy{desired_backup_vel.x, desired_backup_vel.y}; if (!is_zero(desired_backup_vel.x)) {
Vector2f projected_vel; if (is_positive(desired_backup_vel.x)) {
if (!backup_vel_xy.is_zero()) { desired_vel_cms.x = MAX(desired_vel_cms.x, desired_backup_vel.x);
projected_vel = Vector2f{desired_vel_cms.x, desired_vel_cms.y}.projected(Vector2f{desired_backup_vel.x, desired_backup_vel.y}); } else {
desired_vel_cms.x = MIN(desired_vel_cms.x, desired_backup_vel.x);
} }
// subtract this projection since we are already going in that direction }
desired_vel_cms -= Vector3f{projected_vel.x, projected_vel.y, 0.0f}; if (!is_zero(desired_backup_vel.y)) {
desired_vel_cms += Vector3f{desired_backup_vel.x, desired_backup_vel.y, 0.0f}; if (is_positive(desired_backup_vel.y)) {
// let user take control vertically if they are backing away at a greater speed than what we have calculated based on limits desired_vel_cms.y = MAX(desired_vel_cms.y, desired_backup_vel.y);
if (!is_negative(desired_backup_vel.z)) { } else {
desired_vel_cms.y = MIN(desired_vel_cms.y, desired_backup_vel.y);
}
}
if (!is_zero(desired_backup_vel.z)) {
if (is_positive(desired_backup_vel.z)) {
desired_vel_cms.z = MAX(desired_vel_cms.z, desired_backup_vel.z); desired_vel_cms.z = MAX(desired_vel_cms.z, desired_backup_vel.z);
} else { } else {
desired_vel_cms.z = MIN(desired_vel_cms.z, desired_backup_vel.z); desired_vel_cms.z = MIN(desired_vel_cms.z, desired_backup_vel.z);
} }
} else {
// back away to stopping position
desired_vel_cms = desired_backup_vel;
} }
} }
} }