AC_Loiter: Make avoidance optional while using loiter controller

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

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;