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_Loiter: Make avoidance optional while using loiter controller

Browse Source
This commit is contained in:
Rishabh 2021-03-16 00:13:21 +05:30 committed by Randy Mackay
parent bbd32844d1
commit 711b497cc3
2 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -207,7 +207,7 @@ float AC_Loiter::get_angle_max_cd() const
}
/// run the loiter controller
void AC_Loiter::update()
void AC_Loiter::update(bool avoidance_on)
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
@ -219,7 +219,7 @@ void AC_Loiter::update()
_pos_control.set_max_speed_xy(_speed_cms);
_pos_control.set_max_accel_xy(_accel_cmss);
calc_desired_velocity(dt);
calc_desired_velocity(dt, avoidance_on);
_pos_control.update_xy_controller();
}
@ -232,7 +232,7 @@ void AC_Loiter::sanity_check_params()
/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_Loiter::calc_desired_velocity(float nav_dt)
void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
{
float ekfGndSpdLimit, ekfNavVelGainScaler;
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
@ -303,13 +303,15 @@ void AC_Loiter::calc_desired_velocity(float nav_dt)
desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
}
// Limit the velocity to prevent fence violations
// TODO: We need to also limit the _desired_accel
AC_Avoid *_avoid = AP::ac_avoid();
if (_avoid != nullptr) {
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};
if (avoidance_on) {
// Limit the velocity to prevent fence violations
// TODO: We need to also limit the _desired_accel
AC_Avoid *_avoid = AP::ac_avoid();
if (_avoid != nullptr) {
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

4
libraries/AC_WPNav/AC_Loiter.h
View File

@ -48,7 +48,7 @@ public:
float get_angle_max_cd() const;
/// run the loiter controller
void update();
void update(bool avoidance_on = true);
/// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _pos_control.get_roll(); }
@ -63,7 +63,7 @@ protected:
/// updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void calc_desired_velocity(float nav_dt);
void calc_desired_velocity(float nav_dt, bool avoidance_on = true);
// references and pointers to external libraries
const AP_InertialNav& _inav;