From 711b497cc3b192022caaa163459c0611ccd5a30f Mon Sep 17 00:00:00 2001 From: Rishabh Date: Tue, 16 Mar 2021 00:13:21 +0530 Subject: [PATCH] AC_Loiter: Make avoidance optional while using loiter controller --- libraries/AC_WPNav/AC_Loiter.cpp | 22 ++++++++++++---------- libraries/AC_WPNav/AC_Loiter.h | 4 ++-- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 64e2eb56fb..b4c87554c9 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -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 diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 694d01845b..a38311f0b7 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -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;