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

AC_WPNav: Adapt to 3-D Avoidance changes

This commit is contained in:
Rishabh 2021-01-27 21:52:24 +05:30 committed by Randy Mackay
parent f3d825bebe
commit 401d365491
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -307,7 +307,9 @@ void AC_Loiter::calc_desired_velocity(float nav_dt)
// TODO: We need to also limit the _desired_accel
AC_Avoid *_avoid = AP::ac_avoid();
if (_avoid != nullptr) {
_avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _accel_cmss, desired_vel, nav_dt);
Vector3f avoidance_vel_3d{desired_vel.x, desired_vel.y, 0.0f};
_avoid->adjust_velocity(avoidance_vel_3d, _pos_control.get_pos_xy_p().kP(), _accel_cmss, _pos_control.get_pos_z_p().kP(), _pos_control.get_max_accel_z(), nav_dt);
desired_vel = Vector2f{avoidance_vel_3d.x, avoidance_vel_3d.y};
}
// send adjusted feed forward acceleration and velocity back to the Position Controller