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_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

31
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -194,25 +194,28 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel
desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms;
}
if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) {
// project desired velocity towards backup velocity horizontally
const Vector2f backup_vel_xy{desired_backup_vel.x, desired_backup_vel.y};
Vector2f projected_vel;
if (!backup_vel_xy.is_zero()) {
projected_vel = Vector2f{desired_vel_cms.x, desired_vel_cms.y}.projected(Vector2f{desired_backup_vel.x, desired_backup_vel.y});
// let user take control if they are backing away at a greater speed than what we have calculated
// 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".
if (!is_zero(desired_backup_vel.x)) {
if (is_positive(desired_backup_vel.x)) {
desired_vel_cms.x = MAX(desired_vel_cms.x, desired_backup_vel.x);
} 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};
desired_vel_cms += Vector3f{desired_backup_vel.x, desired_backup_vel.y, 0.0f};
// let user take control vertically if they are backing away at a greater speed than what we have calculated based on limits
if (!is_negative(desired_backup_vel.z)) {
}
if (!is_zero(desired_backup_vel.y)) {
if (is_positive(desired_backup_vel.y)) {
desired_vel_cms.y = MAX(desired_vel_cms.y, desired_backup_vel.y);
} 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);
} else {
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;
}
}
}