Sub: integrate AC_Loiter

ArduSub/Parameters.cpp

@@ -445,6 +445,10 @@ const AP_Param::Info Sub::var_info[] = {
     // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
     GOBJECT(wp_nav, "WPNAV_",       AC_WPNav),
 
+    // @Group: LOIT_
+    // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
+    GOBJECT(loiter_nav, "LOITER_", AC_Loiter),
+
 #if CIRCLE_NAV_ENABLED == ENABLED
     // @Group: CIRCLE_
     // @Path: ../libraries/AC_WPNav/AC_Circle.cpp

ArduSub/Parameters.h

@@ -96,6 +96,7 @@ public:
         k_param_circle_nav, // Disabled
         k_param_avoid, // Relies on proximity and fence
         k_param_NavEKF3,
+        k_param_loiter_nav,
 
 
         // Other external hardware interfaces

ArduSub/Sub.cpp

@@ -50,6 +50,7 @@ Sub::Sub(void)
           attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS),
           pos_control(ahrs_view, inertial_nav, motors, attitude_control),
           wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
+          loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           circle_nav(inertial_nav, ahrs_view, pos_control),
           in_mavlink_delay(false),
           param_loader(var_info),

ArduSub/Sub.h

@@ -64,6 +64,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
 #include <AP_InertialNav/AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
 #include <AC_WPNav/AC_WPNav.h>           // Waypoint navigation library
+#include <AC_WPNav/AC_Loiter.h>
 #include <AC_WPNav/AC_Circle.h>          // circle navigation library
 #include <AC_Fence/AC_Fence.h>           // Fence library
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
@@ -396,6 +397,7 @@ private:
     AC_PosControl_Sub pos_control;
 
     AC_WPNav wp_nav;
+    AC_Loiter loiter_nav;
     AC_Circle circle_nav;
 
     // Reference to the relay object

ArduSub/control_auto.cpp

@@ -602,7 +602,7 @@ float Sub::get_auto_heading(void)
         float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
 
         // Bearing from current position towards intermediate position target (centidegrees)
-        float desired_angle = wp_nav.get_loiter_bearing_to_target();
+        float desired_angle = pos_control.get_bearing_to_target();
 
         float angle_error = wrap_180_cd(desired_angle - track_bearing);
         float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);
@@ -646,7 +646,7 @@ bool Sub::auto_terrain_recover_start()
     mission.stop();
 
     // Reset xy target
-    wp_nav.init_loiter_target();
+    loiter_nav.init_target();
 
     // Reset z axis controller
     pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
@@ -733,7 +733,7 @@ void Sub::auto_terrain_recover_run()
     }
 
     // run loiter controller
-    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     ///////////////////////
     // update xy targets //

ArduSub/control_poshold.cpp

@@ -24,7 +24,7 @@ bool Sub::poshold_init()
 
     // set target to current position
     // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
-    wp_nav.init_loiter_target();
+    loiter_nav.init_target();
 
     last_pilot_heading = ahrs.yaw_sensor;
 
@@ -40,7 +40,7 @@ void Sub::poshold_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
-        wp_nav.init_loiter_target();
+        loiter_nav.init_target();
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
         pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
         return;
@@ -50,7 +50,7 @@ void Sub::poshold_run()
     motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run loiter controller
-    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     ///////////////////////
     // update xy outputs //
@@ -64,7 +64,7 @@ void Sub::poshold_run()
     if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
         lateral_out = pilot_lateral;
         forward_out = pilot_forward;
-        wp_nav.init_loiter_target(); // initialize target to current position after repositioning
+        loiter_nav.init_target(); // initialize target to current position after repositioning
     } else {
         translate_wpnav_rp(lateral_out, forward_out);
     }

ArduSub/system.cpp

@@ -124,6 +124,7 @@ void Sub::init_ardupilot()
 
 #if AVOIDANCE_ENABLED == ENABLED
     wp_nav.set_avoidance(&avoid);
+    loiter_nav.set_avoidance(&avoid);
 #endif
 
     pos_control.set_dt(MAIN_LOOP_SECONDS);